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

Controls - Lateral Tuning - Taco Tune

Browse Source 
Use comma's 'Taco Tune' designed for handling left and right turns.
This commit is contained in:
FrogAi 2024-05-10 12:10:54 -07:00
parent b2e290e186
commit 3ee7b539c3
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
selfdrive/controls/lib/longitudinal_planner.py
View File

@ -61,7 +61,7 @@ class LongitudinalPlanner:
self.solverExecutionTime = 0.0 self.solverExecutionTime = 0.0
@staticmethod @staticmethod
def parse_model(model_msg, model_error): def parse_model(model_msg, model_error, v_ego, taco_tune):
if (len(model_msg.position.x) == 33 and if (len(model_msg.position.x) == 33 and
len(model_msg.velocity.x) == 33 and len(model_msg.velocity.x) == 33 and
len(model_msg.acceleration.x) == 33): len(model_msg.acceleration.x) == 33):
@ -74,6 +74,13 @@ class LongitudinalPlanner:
v = np.zeros(len(T_IDXS_MPC)) v = np.zeros(len(T_IDXS_MPC))
a = np.zeros(len(T_IDXS_MPC)) a = np.zeros(len(T_IDXS_MPC))
j = np.zeros(len(T_IDXS_MPC)) j = np.zeros(len(T_IDXS_MPC))
if taco_tune:
max_lat_accel = interp(v_ego, [5, 10, 20], [1.5, 2.0, 3.0])
curvatures = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.orientationRate.z) / np.clip(v, 0.3, 100.0)
max_v = np.sqrt(max_lat_accel / (np.abs(curvatures) + 1e-3)) - 2.0
v = np.minimum(max_v, v)
return x, v, a, j return x, v, a, j
def update(self, sm, frogpilot_toggles): def update(self, sm, frogpilot_toggles):
@ -118,7 +125,7 @@ class LongitudinalPlanner:
self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, sm['frogpilotPlan'].speedJerk, prev_accel_constraint, personality=sm['controlsState'].personality) self.mpc.set_weights(sm['frogpilotPlan'].accelerationJerk, sm['frogpilotPlan'].speedJerk, prev_accel_constraint, personality=sm['controlsState'].personality)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error, v_ego, frogpilot_toggles.taco_tune)
self.mpc.update(sm['radarState'], sm['frogpilotPlan'].vCruise, x, v, a, j, sm['frogpilotPlan'].tFollow, self.mpc.update(sm['radarState'], sm['frogpilotPlan'].vCruise, x, v, a, j, sm['frogpilotPlan'].tFollow,
frogpilot_toggles, personality=sm['controlsState'].personality) frogpilot_toggles, personality=sm['controlsState'].personality)