zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Fix turnspeedControl

Browse Source
This commit is contained in:
ajouatom 2025-02-14 07:10:22 +09:00
parent fdf0af7a32
commit 4d2e755a6f
1 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
selfdrive/carrot/carrot_man.py
View File

@ -1344,8 +1344,8 @@ class CarrotServ:
distanceTraveled = sm['selfdriveState'].distanceTraveled distanceTraveled = sm['selfdriveState'].distanceTraveled
delta_dist = distanceTraveled - self.totalDistance delta_dist = distanceTraveled - self.totalDistance
self.totalDistance = distanceTraveled self.totalDistance = distanceTraveled
steer_torque = 0.0 #sm['controlsState'].actuators.steer steer_torque = sm['carControl'].actuators.steer
turn_speed_ratio = np.interp(abs(steer_torque), [0.7, 1.0], [1.0, 0.6]) turn_speed_ratio = np.interp(abs(steer_torque), [0.8, 1.0], [1.0, 0.6])
self.turn_speed_ratio = self.turn_speed_ratio * 0.9 + turn_speed_ratio * 0.1 self.turn_speed_ratio = self.turn_speed_ratio * 0.9 + turn_speed_ratio * 0.1
else: else:
v_ego = v_ego_kph = 0 v_ego = v_ego_kph = 0
@ -1445,9 +1445,6 @@ class CarrotServ:
#speed_n_sources.append((self.calculate_current_speed(dist, speed * self.mapTurnSpeedFactor, 0, 1.2), "route")) #speed_n_sources.append((self.calculate_current_speed(dist, speed * self.mapTurnSpeedFactor, 0, 1.2), "route"))
desired_speed, source = min(speed_n_sources, key=lambda x: x[0]) desired_speed, source = min(speed_n_sources, key=lambda x: x[0])
if self.turn_speed_ratio < 1.0:
#desired_speed *= self.turn_speed_ratio
source += "T"
if CS is not None: if CS is not None:
if source != self.source_last: if source != self.source_last:
@ -1462,6 +1459,13 @@ class CarrotServ:
source = "gas" source = "gas"
desired_speed = self.gas_override_speed desired_speed = self.gas_override_speed
if self.turn_speed_ratio < 0.95:
if self.turn_speed_ratio < 0.5:
print("ERROR: turn_speed_ratio < 0.5", self.turn_speed_ratio)
else:
desired_speed *= self.turn_speed_ratio
source += "T"
self.debugText = ""#f"desired={desired_speed:.1f},{source},g={self.gas_override_speed:.0f}" self.debugText = ""#f"desired={desired_speed:.1f},{source},g={self.gas_override_speed:.0f}"
left_spd_sec = 100 left_spd_sec = 100