diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 4a90ae9e..49394a87 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -33,6 +33,16 @@ _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] +def get_longitudinal_personality(sm): + controls_state = sm['controlsState'] + for attr in ('personalityDEPRECATED', 'personality'): + try: + return getattr(controls_state, attr) + except Exception: + pass + return sm['selfdriveState'].personality + + def get_max_accel(v_ego): return np.interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) @@ -335,11 +345,13 @@ class LongitudinalPlanner: except Exception: pass + personality = get_longitudinal_personality(sm) + self.mpc.set_weights(sm['starpilotPlan'].accelerationJerk, sm['starpilotPlan'].dangerJerk, sm['starpilotPlan'].speedJerk, prev_accel_constraint, - personality=sm['selfdriveState'].personality, + personality=personality, v_ego=v_ego, lead_dist=self.lead_dist_f if self.lead_dist_f is not None else lead_dist, uncertainty=uncertainty, @@ -353,7 +365,7 @@ class LongitudinalPlanner: tracking_lead = bool(sm['starpilotPlan'].trackingLead) self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, sm['starpilotPlan'].dangerFactor, sm['starpilotPlan'].tFollow, - personality=sm['selfdriveState'].personality, tracking_lead=tracking_lead) + personality=personality, tracking_lead=tracking_lead) self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) diff --git a/starpilot/controls/lib/starpilot_following.py b/starpilot/controls/lib/starpilot_following.py index eacbe915..95f97236 100644 --- a/starpilot/controls/lib/starpilot_following.py +++ b/starpilot/controls/lib/starpilot_following.py @@ -9,6 +9,16 @@ from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, MAX TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT] PERSONALITY_BP = [20. * CV.KPH_TO_MS, 90. * CV.KPH_TO_MS] + +def get_longitudinal_personality(sm): + controls_state = sm["controlsState"] + for attr in ("personalityDEPRECATED", "personality"): + try: + return getattr(controls_state, attr) + except Exception: + pass + return sm["selfdriveState"].personality + class StarPilotFollowing: def __init__(self, StarPilotPlanner): self.starpilot_planner = StarPilotPlanner @@ -24,6 +34,8 @@ class StarPilotFollowing: self.t_follow = 0 def update(self, long_control_active, v_ego, sm, starpilot_toggles): + personality = get_longitudinal_personality(sm) + if long_control_active and sm["starpilotCarState"].trafficModeEnabled: if sm["carState"].aEgo >= 0: self.base_acceleration_jerk = np.interp(v_ego, TRAFFIC_MODE_BP, starpilot_toggles.traffic_mode_jerk_acceleration) @@ -40,21 +52,21 @@ class StarPilotFollowing: starpilot_toggles.aggressive_jerk_acceleration, starpilot_toggles.aggressive_jerk_danger, starpilot_toggles.aggressive_jerk_speed, starpilot_toggles.standard_jerk_acceleration, starpilot_toggles.standard_jerk_danger, starpilot_toggles.standard_jerk_speed, starpilot_toggles.relaxed_jerk_acceleration, starpilot_toggles.relaxed_jerk_danger, starpilot_toggles.relaxed_jerk_speed, - starpilot_toggles.custom_personalities, sm["selfdriveState"].personality + starpilot_toggles.custom_personalities, personality ) else: self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor( starpilot_toggles.aggressive_jerk_deceleration, starpilot_toggles.aggressive_jerk_danger, starpilot_toggles.aggressive_jerk_speed_decrease, starpilot_toggles.standard_jerk_deceleration, starpilot_toggles.standard_jerk_danger, starpilot_toggles.standard_jerk_speed_decrease, starpilot_toggles.relaxed_jerk_deceleration, starpilot_toggles.relaxed_jerk_danger, starpilot_toggles.relaxed_jerk_speed_decrease, - starpilot_toggles.custom_personalities, sm["selfdriveState"].personality + starpilot_toggles.custom_personalities, personality ) self.t_follow = get_T_FOLLOW( starpilot_toggles.aggressive_follow, starpilot_toggles.standard_follow, starpilot_toggles.relaxed_follow, - starpilot_toggles.custom_personalities, sm["selfdriveState"].personality + starpilot_toggles.custom_personalities, personality ) if isinstance(self.t_follow, (list, tuple)): self.t_follow = float(np.interp(v_ego, PERSONALITY_BP, self.t_follow))