diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 714e182..6dd102a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -61,7 +61,7 @@ class LongitudinalPlanner: self.solverExecutionTime = 0.0 @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 len(model_msg.velocity.x) == 33 and len(model_msg.acceleration.x) == 33): @@ -74,6 +74,13 @@ class LongitudinalPlanner: v = np.zeros(len(T_IDXS_MPC)) a = 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 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_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) 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, frogpilot_toggles, personality=sm['controlsState'].personality)