diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 20555395f..accad7d05 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -46,6 +46,30 @@ def get_gm_hud_set_speed(set_speed_ms: float, starpilot_toggles) -> float: return spoofed_speed +def get_torque_control_params(CP, torque_params, starpilot_toggles, use_live_params: bool) -> tuple[float, float, float]: + torque_tune = CP.lateralTuning.torque + lat_accel_factor = torque_tune.latAccelFactor + lat_accel_offset = torque_tune.latAccelOffset + friction = torque_tune.friction + + use_custom_lat_accel = getattr(starpilot_toggles, "use_custom_latAccelFactor", False) + use_custom_friction = getattr(starpilot_toggles, "use_custom_friction", False) + + if use_live_params: + if not use_custom_lat_accel: + lat_accel_factor = torque_params.latAccelFactorFiltered + lat_accel_offset = torque_params.latAccelOffsetFiltered + if not use_custom_friction: + friction = torque_params.frictionCoefficientFiltered + + if use_custom_lat_accel: + lat_accel_factor = starpilot_toggles.latAccelFactor + if use_custom_friction: + friction = starpilot_toggles.friction + + return lat_accel_factor, lat_accel_offset, friction + + class Controls: def __init__(self) -> None: self.params = Params() @@ -82,6 +106,8 @@ class Controls: self.sm = self.sm.extend(['liveDelay', 'starpilotCarState', 'starpilotPlan']) self.starpilot_toggles = get_starpilot_toggles() + self.ecu_disable_failed = False + self.ecu_disable_failed_checked = not self.CP.openpilotLongitudinalControl if self.CP.lateralTuning.which() == "torque" and (self.starpilot_toggles.nnff or self.starpilot_toggles.nnff_lite): self.LaC = LatControlNNFF(self.CP, self.CI, DT_CTRL) @@ -102,6 +128,16 @@ class Controls: self.starpilot_toggles = get_starpilot_toggles(self.sm) + def update_ecu_disable_failed(self): + if self.ecu_disable_failed_checked: + return + + # ControlsReady is set after CarInterface.init(), where Hyundai ECU disable + # writes EcuDisableFailed. Once init has completed, the value is stable. + if self.params.get_bool("ControlsReady"): + self.ecu_disable_failed = self.params.get_bool("EcuDisableFailed") + self.ecu_disable_failed_checked = True + def state_control(self): CS = self.sm['carState'] @@ -121,9 +157,15 @@ class Controls: # Update Torque Params if self.CP.lateralTuning.which() == 'torque': torque_params = self.sm['liveTorqueParameters'] - if self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or self.starpilot_toggles.force_auto_tune): - self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered, - torque_params.frictionCoefficientFiltered) + force_auto_tune = getattr(self.starpilot_toggles, "force_auto_tune", False) + use_live_params = self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or force_auto_tune) + use_custom_torque_params = ( + getattr(self.starpilot_toggles, "use_custom_latAccelFactor", False) or + getattr(self.starpilot_toggles, "use_custom_friction", False) + ) + if use_live_params or use_custom_torque_params: + lat_accel_factor, lat_accel_offset, friction = get_torque_control_params(self.CP, torque_params, self.starpilot_toggles, use_live_params) + self.LaC.update_live_torque_params(lat_accel_factor, lat_accel_offset, friction) long_plan = self.sm['longitudinalPlan'] model_v2 = self.sm['modelV2'] @@ -140,8 +182,8 @@ class Controls: self.sm['starpilotPlan'].lateralCheck) # EcuDisableFailed is set when car started in READY mode (ECU disable was rejected) # Disable longitudinal so stock ACC works instead - ecu_disable_failed = self.params.get_bool("EcuDisableFailed") - CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and not ecu_disable_failed + self.update_ecu_disable_failed() + CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed actuators = CC.actuators actuators.longControlState = self.LoC.long_control_state