diff --git a/opendbc/car/car.capnp b/opendbc/car/car.capnp index 8d26c8eedb5..2ca51cea2d2 100644 --- a/opendbc/car/car.capnp +++ b/opendbc/car/car.capnp @@ -204,6 +204,7 @@ struct CarState { lowSpeedAlert @56 :Bool; # lost steering control due to a dynamic min steering speed blockPcmEnable @60 :Bool; # whether to allow PCM to enable this frame carNotReady @61 :Bool; # car is transiently refusing engagement, used to prevent a fault if engaged + steerTimeLimit @62 :Bool; # steering will soon be refused; inferred from time spent steering, not reported by the car # cruise state cruiseState @10 :CruiseState; diff --git a/opendbc/car/volkswagen/carcontroller.py b/opendbc/car/volkswagen/carcontroller.py index 4d4f394fdeb..d7025fdb5bb 100644 --- a/opendbc/car/volkswagen/carcontroller.py +++ b/opendbc/car/volkswagen/carcontroller.py @@ -15,13 +15,24 @@ class HCAMitigation: """ Manages HCA fault mitigations for VW/Audi EPS racks: * Reduces torque by 1 for a single frame after commanding the same torque value for too long + * For MLB racks: opportunistically disables HCA during low-torque periods before the 6-minute EPS lockout """ - def __init__(self, CCP): + def __init__(self, CCP, steer_timer_mitigation=False): self._max_same_torque_frames = CCP.STEER_TIME_STUCK_TORQUE / (DT_CTRL * CCP.STEER_STEP) self._same_torque_frames = 0 - def update(self, apply_torque, apply_torque_last): + self._steer_timer_mitigation = steer_timer_mitigation + if steer_timer_mitigation: + self._hca_active_frames = 0 + self._hca_inactive_frames = 0 + self._low_torque_frames = 0 + self._low_torque_threshold = CCP.STEER_TIME_LOW_TORQUE + self._frames_mitigation_start = CCP.STEER_TIME_MITIGATION_START / (DT_CTRL * CCP.STEER_STEP) + self._frames_low_torque = CCP.STEER_TIME_LOW_TORQUE_TIME / (DT_CTRL * CCP.STEER_STEP) + self._frames_reset = CCP.STEER_TIME_RESET / (DT_CTRL * CCP.STEER_STEP) + + def update(self, apply_torque, apply_torque_last, desired_torque): if apply_torque != 0 and apply_torque_last == apply_torque: self._same_torque_frames += 1 if self._same_torque_frames > self._max_same_torque_frames: @@ -30,6 +41,22 @@ def update(self, apply_torque, apply_torque_last): else: self._same_torque_frames = 0 + # Disabling lateral controls for ~1.1s will reset the max steer timer on MLB. This reset is done opportunistically + # during sustained periods of low desired torque. In the rare case that no such low desired torque period exists + # there is a warning triggered prior to the hard lockout by CarState.steer_time_limit_warning(). + if self._steer_timer_mitigation: + if self._hca_inactive_frames >= self._frames_reset: + self._hca_active_frames = 0 + + low_torque = abs(desired_torque) <= self._low_torque_threshold + + if low_torque and self._low_torque_frames >= self._frames_low_torque and self._hca_active_frames >= self._frames_mitigation_start: + apply_torque = 0 + + self._low_torque_frames = self._low_torque_frames + 1 if low_torque else 0 + self._hca_inactive_frames = self._hca_inactive_frames + 1 if apply_torque == 0 else 0 + self._hca_active_frames += 1 + return apply_torque @@ -58,7 +85,7 @@ def __init__(self, dbc_names, CP): self.lead_distance_bars_last = None self.distance_bar_frame = 0 self.gra_acc_counter_last = None - self.hca_mitigation = HCAMitigation(self.CCP) + self.hca_mitigation = HCAMitigation(self.CCP, steer_timer_mitigation=bool(CP.flags & VolkswagenFlags.MLB)) def update(self, CC, CS, now_nanos): actuators = CC.actuators @@ -104,11 +131,12 @@ def update(self, CC, CS, now_nanos): self.steering_power_last = steering_power else: + new_torque = 0 if CC.latActive: new_torque = int(round(actuators.torque * self.CCP.STEER_MAX)) apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.CCP) - apply_torque = self.hca_mitigation.update(apply_torque, self.apply_torque_last) + apply_torque = self.hca_mitigation.update(apply_torque, self.apply_torque_last, new_torque) hca_enabled = apply_torque != 0 self.apply_torque_last = apply_torque can_sends.append(self.CCS.create_steering_control(self.packer_pt, self.CAN.pt, apply_torque, hca_enabled)) diff --git a/opendbc/car/volkswagen/carstate.py b/opendbc/car/volkswagen/carstate.py index 3844a39a8d6..b4243b324d7 100644 --- a/opendbc/car/volkswagen/carstate.py +++ b/opendbc/car/volkswagen/carstate.py @@ -1,5 +1,5 @@ from opendbc.can import CANParser -from opendbc.car import Bus, structs +from opendbc.car import Bus, DT_CTRL, structs from opendbc.car.interfaces import CarStateBase from opendbc.car.common.conversions import Conversions as CV from opendbc.car.volkswagen.values import DBC, CanBus, NetworkLocation, TransmissionType, GearShifter, \ @@ -13,8 +13,12 @@ def __init__(self, CP): super().__init__(CP) self.frame = 0 self.eps_init_complete = False + self.hca_active_frames = 0 + self.hca_inactive_frames = 0 self.tsk_recovery_timer = 0 self.CCP = CarControllerParams(CP) + self.frames_steering_reset_detection = (self.CCP.STEER_TIME_RESET - self.CCP.STEER_TIME_RESET_DETECTION_BUFFER) / DT_CTRL + self.frames_steering_time_warning = (self.CCP.STEER_TIME_MAX_ENGAGED - self.CCP.STEER_TIME_WARNING) / DT_CTRL self.button_states = {button.event_type: False for button in self.CCP.BUTTONS} self.esp_hold_confirmation = False self.upscale_lead_car_signal = False @@ -390,6 +394,19 @@ def parse_mlb_mqb_steering_state(self, ret, pt_cp): hca_status = self.CCP.hca_status_values.get(pt_cp.vl["LH_EPS_03"]["EPS_HCA_Status"]) ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status) + if self.CP.flags & VolkswagenFlags.MLB: + ret.steerTimeLimit = self.steer_time_limit_warning(hca_status) + + def steer_time_limit_warning(self, hca_status) -> bool: + # MLB steering racks have a hard 6min max engagement. After that time it will return status = 'rejected' for ~2.0s and not execute torque requests. After + # the ~2.0s lockout period it will return to accepting torque requests. This warning trigger gives advance notice to the driver that steering is about to + # become unavailable so they can take control. This should only fire very rarely as we will opportunistically reset the steering rack in HCAMitigation. + warning = self.hca_active_frames >= self.frames_steering_time_warning + + self.hca_inactive_frames = 0 if hca_status in ("ACTIVE", "ACTIVE_MODE_7") else self.hca_inactive_frames + 1 + self.hca_active_frames = 0 if self.hca_inactive_frames >= self.frames_steering_reset_detection else self.hca_active_frames + 1 + + return warning def update_hca_state(self, hca_status, in_drive=True): # Treat FAULT as temporary for worst likely EPS recovery time, for cars without factory Lane Assist diff --git a/opendbc/car/volkswagen/tests/test_volkswagen.py b/opendbc/car/volkswagen/tests/test_volkswagen.py index 45293ad395c..e39105bb12a 100644 --- a/opendbc/car/volkswagen/tests/test_volkswagen.py +++ b/opendbc/car/volkswagen/tests/test_volkswagen.py @@ -23,11 +23,48 @@ def test_same_torque_mitigation(self): hca_mitigation = HCAMitigation(CCP) for actuator_value in (-CCP.STEER_MAX, -1, 0, 1, CCP.STEER_MAX): - hca_mitigation.update(0, 0) # Reset mitigation state + hca_mitigation.update(0, 0, 0) # Reset mitigation state for frame in range(self.STUCK_TORQUE_FRAMES + 2): should_nudge = actuator_value != 0 and frame == self.STUCK_TORQUE_FRAMES expected_torque = actuator_value - (1, -1)[actuator_value < 0] if should_nudge else actuator_value - assert hca_mitigation.update(actuator_value, actuator_value) == expected_torque, f"{frame=}" + assert hca_mitigation.update(actuator_value, actuator_value, actuator_value) == expected_torque, f"{frame=}" + + def test_eps_timer_reset(self): + """The mitigation respects start time threshold, aborts on real steering requests, and resets when expected.""" + hca_mitigation = HCAMitigation(CCP, steer_timer_mitigation=True) + mitigation_start_calls = round(CCP.STEER_TIME_MITIGATION_START / (DT_CTRL * CCP.STEER_STEP)) + low_torque_calls = round(CCP.STEER_TIME_LOW_TORQUE_TIME / (DT_CTRL * CCP.STEER_STEP)) + reset_calls = round(CCP.STEER_TIME_RESET / (DT_CTRL * CCP.STEER_STEP)) + + low_torque = CCP.STEER_TIME_LOW_TORQUE + apply_torque = 0 + + for _ in range(mitigation_start_calls): + apply_torque = hca_mitigation.update(low_torque, apply_torque, low_torque) + assert apply_torque != 0, "mitigation must not engage before STEER_TIME_MITIGATION_START" + + # Reset aborted due to desired torque above threshold + for _ in range(reset_calls - 1): + apply_torque = hca_mitigation.update(low_torque, apply_torque, low_torque) + assert apply_torque == 0, "sustained low torque should start reset" + apply_torque = hca_mitigation.update(low_torque, apply_torque, low_torque + 1) + assert apply_torque != 0, "reset must abort when desired torque is above threshold" + + # Reset should not start during torque above threshold + for _ in range(low_torque_calls + 1): + apply_torque = hca_mitigation.update(low_torque + 1, apply_torque, low_torque + 1) + assert apply_torque != 0, "reset must not trigger while the model is requesting high torque" + + # Reset successful + for _ in range(low_torque_calls): + apply_torque = hca_mitigation.update(low_torque, apply_torque, low_torque) + assert apply_torque != 0, "reset should not start until low torque is observed for required period" + for _ in range(reset_calls): + apply_torque = hca_mitigation.update(low_torque, apply_torque, low_torque) + assert apply_torque == 0, "sustained low torque should zero the output to reset the EPS timer" + apply_torque = hca_mitigation.update(low_torque, apply_torque, low_torque) + assert apply_torque == low_torque, "EPS timer reset should complete and release steering" + class TestVolkswagenPlatformConfigs(unittest.TestCase): def test_spare_part_fw_pattern(self): diff --git a/opendbc/car/volkswagen/values.py b/opendbc/car/volkswagen/values.py index 0c023014cf3..d9d47f9449a 100644 --- a/opendbc/car/volkswagen/values.py +++ b/opendbc/car/volkswagen/values.py @@ -62,6 +62,13 @@ class CarControllerParams: STEER_DRIVER_FACTOR = 1 # from dbc STEER_TIME_STUCK_TORQUE = 1.9 # EPS limits same torque to 6 seconds, reset timer 3x within that period + STEER_TIME_RESET = 1.1 # MLB-only: HCA must stay disabled this long for the EPS to reset its steer timer (sec) + STEER_TIME_RESET_DETECTION_BUFFER = 0.05 # MLB-only: Detection uses LH_EPS_03 whereas actuation is HCA_01 so allow some buffer in detection (sec) + STEER_TIME_MAX_ENGAGED = 360. # MLB-only: Maximum EPS engagement time before lockout will reject requests (sec) + STEER_TIME_WARNING = 5. # MLB-only: How long before max engaged steer time should warning fire (sec) + STEER_TIME_MITIGATION_START = 240. # MLB-only: EPS engaged time before attempting opportunistic reset (sec) + STEER_TIME_LOW_TORQUE = 60 # MLB-only: Desired torque must be less than this before and during reset (centi-Nm) + STEER_TIME_LOW_TORQUE_TIME = 0.5 # MLB-only: How long to observe low torque for before starting the reset (sec) DEFAULT_MIN_STEER_SPEED = 0.4 # m/s, newer EPS racks fault below this speed, don't show a low speed alert diff --git a/opendbc/dbc/vw_mlb.dbc b/opendbc/dbc/vw_mlb.dbc index 4f7bb05a0b5..9aa443bb381 100644 --- a/opendbc/dbc/vw_mlb.dbc +++ b/opendbc/dbc/vw_mlb.dbc @@ -1766,7 +1766,7 @@ BO_ 1318 BCM: 8 XXX SG_ BLINKER_RIGHT : 35|1@0+ (1,0) [0|1] "" XXX SG_ UNKNOWN_DRIVER_DOOR_RELATED : 43|1@0+ (1,0) [0|1] "" XXX -VAL_ 159 EPS_HCA_Status 0 "disabled" 1 "initializing" 2 "fault" 3 "ready" 4 "rejected" 5 "active" ; +VAL_ 159 EPS_HCA_Status 0 "disabled" 1 "initializing" 2 "fault" 3 "ready" 4 "rejected" 5 "active" 7 "active_mode_7" ; VAL_ 269 ACC_Freigabe_Momentenanf 0 "Momanf_nicht_freigegeben" 1 "Momanf_freigegeben" ; VAL_ 269 ACC_Freigabe_Verzanf 0 "Verzanf_nicht_freigegeben" 1 "Verzanf_freigegeben" ; VAL_ 269 ACC_Getriebestellung_P 0 "P_Stellung_nicht_angefordert" 1 "Anforderung_P_Stellung" ;