diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index c9897500a5..0953ef31a4 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -10,8 +10,6 @@ from openpilot.common.swaglog import cloudlog from openpilot.selfdrive.modeld.constants import index_function from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU -from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelPersonalityController -from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_personality.dynamic_follow import FollowDistanceController if __name__ == '__main__': # generating code from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver else: @@ -230,8 +228,6 @@ class LongitudinalMpc: self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) self.reset() self.source = SOURCES[2] - self.accel_controller = AccelPersonalityController() - self.dynamic_follow = FollowDistanceController() def reset(self): # self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N) @@ -331,28 +327,13 @@ class LongitudinalMpc: lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau) return lead_xv - def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard): + def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard, a_cruise_min_override=None, t_follow_override=None): + t_follow = t_follow_override if t_follow_override is not None else get_T_FOLLOW(personality) + a_cruise_min = a_cruise_min_override if a_cruise_min_override is not None else CRUISE_MIN_ACCEL + v_ego = self.x0[1] - - if self.dynamic_follow.is_enabled(): - t_follow = self.dynamic_follow.get_follow_distance_multiplier(v_ego) - #print(f"DEBUG: dynamic_follow enabled, t_follow={t_follow:.3f}, v_ego={v_ego:.2f}, v_cruise={v_cruise:.2f}") - else: - t_follow = get_T_FOLLOW(personality) - #print(f"DEBUG: dynamic_follow disabled, using personality t_follow={t_follow:.3f}, personality={personality}") - self.status = radarstate.leadOne.status or radarstate.leadTwo.status - # Get acceleration limits - if self.accel_controller.is_enabled(): - min_accel = self.accel_controller.get_min_accel(v_ego) - #print(f"DEBUG: accel_enabled=True, min_accel={min_accel:.3f}") - else: - min_accel = CRUISE_MIN_ACCEL - #print(f"DEBUG: accel_enabled=False, using stock min_accel={min_accel}") - - a_cruise_min = min_accel - lead_xv_0 = self.process_lead(radarstate.leadOne) lead_xv_1 = self.process_lead(radarstate.leadTwo) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 566204cd4a..fed0c3e760 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -124,10 +124,8 @@ class LongitudinalPlanner(LongitudinalPlannerSP): prev_accel_constraint = not (reset_state or sm['carState'].standstill) if mode == 'acc': - if self.accel_controller.is_enabled(): - max_accel = self.accel_controller.get_max_accel(v_ego) - #print(f"Vibe personality active - max accel: {max_accel:.3f}") - accel_clip = [ACCEL_MIN, max_accel] + if sp_accel_clip := LongitudinalPlannerSP.get_accel_clip(self, v_ego, mode): + accel_clip = sp_accel_clip else: accel_clip = [ACCEL_MIN, get_max_accel(v_ego)] @@ -160,7 +158,9 @@ class LongitudinalPlanner(LongitudinalPlannerSP): self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality) + a_cruise_min_override = LongitudinalPlannerSP.get_cruise_min_accel(self, v_ego) + t_follow_override = LongitudinalPlannerSP.get_t_follow(self, v_ego) + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality, a_cruise_min_override=a_cruise_min_override, t_follow_override=t_follow_override) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index bf23fbf2f4..a2e1e11a2b 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -11,6 +11,7 @@ from openpilot.common.realtime import DT_MDL from openpilot.common.params import Params AccelPersonality = custom.LongitudinalPlanSP.AccelerationPersonality +ACCEL_PERSONALITY_OPTIONS = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport] # Acceleration Profiles MAX_ACCEL_PROFILES = { @@ -41,43 +42,33 @@ class AccelPersonalityController: def __init__(self): self.params = Params() self.frame = 0 - self.accel_personality = AccelPersonality.normal self.last_max_accel = 2.0 self.last_min_accel = -0.01 self.first_run = True - self.param_keys = {'personality': 'AccelPersonality', 'enabled': 'AccelPersonalityEnabled'} - self._load_personality_from_params() - - def _load_personality_from_params(self): - try: - saved = self.params.get(self.param_keys['personality']) - if saved is not None: - val = int(saved) - if val in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]: - self.accel_personality = val - except (ValueError, TypeError): - pass - - def _update_from_params(self): - if self.frame % int(1. / DT_MDL) == 0: - self._load_personality_from_params() + self._accel_personality = self.params.get('AccelPersonality') or AccelPersonality.normal + self._enabled = self.params.get_bool('AccelPersonalityEnabled') def update(self, sm=None): self.frame += 1 - self._update_from_params() + if self.frame % int(1.0 / DT_MDL) == 0: + self._accel_personality = self.params.get('AccelPersonality') or AccelPersonality.normal + self._enabled = self.params.get_bool('AccelPersonalityEnabled') + + @property + def accel_personality(self) -> int: + return self._accel_personality def get_accel_personality(self) -> int: - self._update_from_params() - return int(self.accel_personality) + return int(self._accel_personality) def set_accel_personality(self, personality: int): - if personality in [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport]: - self.accel_personality = personality - self.params.put(self.param_keys['personality'], str(personality)) + if personality in ACCEL_PERSONALITY_OPTIONS: + self._accel_personality = personality + self.params.put('AccelPersonality', personality) def cycle_accel_personality(self) -> int: - personality = [AccelPersonality.eco, AccelPersonality.normal, AccelPersonality.sport] - next_personality = personality[(personality.index(self.accel_personality) + 1) % len(personality)] + current = self._accel_personality + next_personality = ACCEL_PERSONALITY_OPTIONS[(ACCEL_PERSONALITY_OPTIONS.index(current) + 1) % len(ACCEL_PERSONALITY_OPTIONS)] self.set_accel_personality(next_personality) return int(next_personality) @@ -92,8 +83,8 @@ class AccelPersonalityController: return float(target_min), float(target_max) # Smoothing - self.last_max_accel = (ACCEL_SMOOTH_ALPHA * target_max + (1 - ACCEL_SMOOTH_ALPHA) * self.last_max_accel) - smoothed_decel = (DECEL_SMOOTH_ALPHA * target_min + (1 - DECEL_SMOOTH_ALPHA) * self.last_min_accel) + self.last_max_accel = ACCEL_SMOOTH_ALPHA * target_max + (1 - ACCEL_SMOOTH_ALPHA) * self.last_max_accel + smoothed_decel = DECEL_SMOOTH_ALPHA * target_min + (1 - DECEL_SMOOTH_ALPHA) * self.last_min_accel # Rate Limiting (Asymmetric) raw_change = smoothed_decel - self.last_min_accel @@ -124,18 +115,20 @@ class AccelPersonalityController: return self.get_accel_limits(v_ego)[1] def is_enabled(self) -> bool: - return self.params.get_bool(self.param_keys['enabled']) + return self._enabled def set_enabled(self, enabled: bool): - self.params.put_bool(self.param_keys['enabled'], enabled) + self._enabled = enabled + self.params.put_bool('AccelPersonalityEnabled', enabled) def toggle_enabled(self) -> bool: - current = self.is_enabled() + current = self._enabled self.set_enabled(not current) return not current def reset(self): - self.accel_personality = AccelPersonality.normal + self._accel_personality = AccelPersonality.normal + self.params.put('AccelPersonality', AccelPersonality.normal) self.frame = 0 self.last_max_accel = 2.0 self.last_min_accel = -0.01 diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_follow.py b/sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_follow.py index 763a92d02a..7c86a333f3 100644 --- a/sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_follow.py +++ b/sunnypilot/selfdrive/controls/lib/dynamic_personality/dynamic_follow.py @@ -31,93 +31,67 @@ class FollowDistanceController: def __init__(self): self.params = Params() self.frame = 0 - self.personality = LongPersonality.standard self.current_multiplier = None self.first_run = True self.personality_change_cooldown = 0 self.personality_cooldown_frames = int(PERSONALITY_CHANGE_COOLDOWN_S / DT_MDL) - self._load_personality() - - def _load_personality(self): - try: - saved = self.params.get('LongitudinalPersonality') - if saved is not None: - val = int(saved) - if val in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]: - self.personality = val - except (ValueError, TypeError): - pass - - def _update_from_params(self): - if self.frame % int(1. / DT_MDL) != 0: - return - - if self.personality_change_cooldown > 0: - self.personality_change_cooldown -= 1 - return - - try: - param = self.params.get('LongitudinalPersonality') - if param is not None: - val = int(param) - if val in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]: - if val != self.personality: - self.personality = val - self.personality_change_cooldown = self.personality_cooldown_frames - except (ValueError, TypeError): - pass + self._personality = self.params.get('LongitudinalPersonality') or LongPersonality.standard + self._enabled = self.params.get_bool('DynamicFollow') def _get_smoothing_factor(self, v_ego: float) -> float: speed_factor = np.clip(v_ego / SMOOTHING_SPEED_THRESHOLD, 0.3, 1.0) return SMOOTHING_BASE + (SMOOTHING_RANGE * speed_factor) def is_enabled(self) -> bool: - return self.params.get_bool('DynamicFollow') + return self._enabled def set_enabled(self, enabled: bool): + self._enabled = enabled self.params.put_bool('DynamicFollow', enabled) def toggle(self) -> bool: - enabled = self.is_enabled() - self.set_enabled(not enabled) - return not enabled + current = self._enabled + self.set_enabled(not current) + return not current + + @property + def personality(self) -> int: + return self._personality def get_personality(self) -> int: - self._update_from_params() - return int(self.personality) + return int(self._personality) def set_personality(self, personality: int): if personality not in [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive]: return - - self.personality = personality - self.params.put('LongitudinalPersonality', str(personality)) + self._personality = personality + self.params.put('LongitudinalPersonality', personality) self.personality_change_cooldown = self.personality_cooldown_frames def cycle_personality(self) -> int: personalities = [LongPersonality.relaxed, LongPersonality.standard, LongPersonality.aggressive] - current_idx = personalities.index(self.personality) + current_idx = personalities.index(self._personality) next_personality = personalities[(current_idx + 1) % len(personalities)] self.set_personality(next_personality) return int(next_personality) def get_follow_distance_multiplier(self, v_ego: float) -> float: - self._update_from_params() v_ego = max(0.0, v_ego) - target = float(np.interp(v_ego, FOLLOW_BREAKPOINTS, FOLLOW_PROFILES[self.personality])) + target = float(np.interp(v_ego, FOLLOW_BREAKPOINTS, FOLLOW_PROFILES[self._personality])) if self.first_run: self.current_multiplier = target self.first_run = False return self.current_multiplier - #exponential smoothing with speedadaptive factor + # exponential smoothing with speedadaptive factor alpha = self._get_smoothing_factor(v_ego) self.current_multiplier = alpha * self.current_multiplier + (1.0 - alpha) * target return self.current_multiplier def reset(self): - self.personality = LongPersonality.standard + self._personality = LongPersonality.standard + self.params.put('LongitudinalPersonality', LongPersonality.standard) self.frame = 0 self.current_multiplier = None self.first_run = True @@ -125,4 +99,8 @@ class FollowDistanceController: def update(self): self.frame += 1 - self._update_from_params() + if self.personality_change_cooldown > 0: + self.personality_change_cooldown -= 1 + if self.frame % int(1.0 / DT_MDL) == 0: + self._personality = self.params.get('LongitudinalPersonality') or LongPersonality.standard + self._enabled = self.params.get_bool('DynamicFollow') diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index cd71e437fa..2f000aad40 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -18,6 +18,9 @@ from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP from openpilot.sunnypilot.models.helpers import get_active_bundle from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelPersonalityController +from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_personality.dynamic_follow import FollowDistanceController +from opendbc.car.interfaces import ACCEL_MIN + DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource @@ -28,6 +31,7 @@ class LongitudinalPlannerSP: self.resolver = SpeedLimitResolver() self.dec = DynamicExperimentalController(CP, mpc) self.accel_controller = AccelPersonalityController() + self.dynamic_follow = FollowDistanceController() self.scc = SmartCruiseControl() self.resolver = SpeedLimitResolver() self.sla = SpeedLimitAssist(CP, CP_SP) @@ -35,8 +39,8 @@ class LongitudinalPlannerSP: self.source = LongitudinalPlanSource.cruise self.e2e_alerts_helper = E2EAlertsHelper() - self.output_v_target = 0. - self.output_a_target = 0. + self.output_v_target = 0.0 + self.output_a_target = 0.0 @property def mlsim(self) -> bool: @@ -49,6 +53,21 @@ class LongitudinalPlannerSP: return self.dec.mode() + def get_accel_clip(self, v_ego: float, mode: str) -> list[float] | None: + if mode == 'acc' and self.accel_controller.is_enabled(): + return [ACCEL_MIN, self.accel_controller.get_max_accel(v_ego)] + return None + + def get_cruise_min_accel(self, v_ego: float) -> float | None: + if self.accel_controller.is_enabled(): + return self.accel_controller.get_min_accel(v_ego) + return None + + def get_t_follow(self, v_ego: float) -> float | None: + if self.dynamic_follow.is_enabled(): + return self.dynamic_follow.get_follow_distance_multiplier(v_ego) + return None + def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]: CS = sm['carState'] v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)