diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 3b897c0c160de4..4db5606217283b 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -21,8 +21,9 @@ def __init__(self, dbc_name, CP, VM): self.apply_steer_last = 0 self.gra_acc_counter_last = None self.frame = 0 - self.hcaSameTorqueCount = 0 - self.hcaEnabledFrameCount = 0 + self.eps_timer_soft_disable_alert = False + self.hca_frame_timer_running = 0 + self.hca_frame_same_torque = 0 def update(self, CC, CS, ext_bus, now_nanos): actuators = CC.actuators @@ -38,36 +39,31 @@ def update(self, CC, CS, ext_bus, now_nanos): # * Don't send > 3.00 Newton-meters torque # * Don't send the same torque for > 6 seconds # * Don't send uninterrupted steering for > 360 seconds - # One frame of HCA disabled is enough to reset the timer, without zeroing the - # torque value. Do that anytime we happen to have 0 torque, or failing that, - # when exceeding ~1/3 the 360 second timer. + # MQB racks reset the uninterrupted steering timer after a single frame + # of HCA disabled; this is done whenever output happens to be zero. if CC.latActive: new_steer = int(round(actuators.steer * self.CCP.STEER_MAX)) apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.CCP) - if apply_steer == 0: - hcaEnabled = False - self.hcaEnabledFrameCount = 0 + self.hca_frame_timer_running += self.CCP.STEER_STEP + if self.apply_steer_last == apply_steer: + self.hca_frame_same_torque += self.CCP.STEER_STEP + if self.hca_frame_same_torque > self.CCP.STEER_TIME_STUCK_TORQUE / DT_CTRL: + apply_steer -= (1, -1)[apply_steer < 0] + self.hca_frame_same_torque = 0 else: - self.hcaEnabledFrameCount += 1 - if self.hcaEnabledFrameCount >= 118 * (100 / self.CCP.STEER_STEP): # 118s - hcaEnabled = False - self.hcaEnabledFrameCount = 0 - else: - hcaEnabled = True - if self.apply_steer_last == apply_steer: - self.hcaSameTorqueCount += 1 - if self.hcaSameTorqueCount > 1.9 * (100 / self.CCP.STEER_STEP): # 1.9s - apply_steer -= (1, -1)[apply_steer < 0] - self.hcaSameTorqueCount = 0 - else: - self.hcaSameTorqueCount = 0 + self.hca_frame_same_torque = 0 + hca_enabled = abs(apply_steer) > 0 else: - hcaEnabled = False + hca_enabled = False apply_steer = 0 + if not hca_enabled: + self.hca_frame_timer_running = 0 + + self.eps_timer_soft_disable_alert = self.hca_frame_timer_running > self.CCP.STEER_TIME_ALERT / DT_CTRL self.apply_steer_last = apply_steer - can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer, hcaEnabled)) + can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer, hca_enabled)) # **** Acceleration Controls ******************************************** # @@ -110,4 +106,4 @@ def update(self, CC, CS, ext_bus, now_nanos): self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"] self.frame += 1 - return new_actuators, can_sends + return new_actuators, can_sends, self.eps_timer_soft_disable_alert diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 874b85e68ed056..1d6848b48eebba 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -20,6 +20,8 @@ def __init__(self, CP, CarController, CarState): self.ext_bus = CANBUS.cam self.cp_ext = self.cp_cam + self.eps_timer_soft_disable_alert = False + @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs): ret.carName = "volkswagen" @@ -242,9 +244,13 @@ def _update(self, c): if c.enabled and ret.vEgo < self.CP.minEnableSpeed: events.add(EventName.speedTooLow) + if self.eps_timer_soft_disable_alert: + events.add(EventName.steerTimeLimit) + ret.events = events.to_msg() return ret def apply(self, c, now_nanos): - return self.CC.update(c, self.CS, self.ext_bus, now_nanos) + new_actuators, can_sends, self.eps_timer_soft_disable_alert = self.CC.update(c, self.CS, self.ext_bus, now_nanos) + return new_actuators, can_sends diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 65c32023dc6cb4..b8fb173cfea109 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -19,20 +19,25 @@ class CarControllerParams: - STEER_STEP = 2 # HCA_01/HCA_1 message frequency 50Hz - ACC_CONTROL_STEP = 2 # ACC_06/ACC_07/ACC_System frequency 50Hz + STEER_STEP = 2 # HCA_01/HCA_1 message frequency 50Hz + ACC_CONTROL_STEP = 2 # ACC_06/ACC_07/ACC_System frequency 50Hz - ACCEL_MAX = 2.0 # 2.0 m/s max acceleration - ACCEL_MIN = -3.5 # 3.5 m/s max deceleration + # Documented lateral limits: 3.00 Nm max, rate of change 5.00 Nm/sec. + # MQB vs PQ maximums are shared, but rate-of-change limited differently + # based on safety requirements driven by lateral accel testing. - def __init__(self, CP): - # Documented lateral limits: 3.00 Nm max, rate of change 5.00 Nm/sec. - # MQB vs PQ maximums are shared, but rate-of-change limited differently - # based on safety requirements driven by lateral accel testing. - self.STEER_MAX = 300 # Max heading control assist torque 3.00 Nm - self.STEER_DRIVER_MULTIPLIER = 3 # weight driver torque heavily - self.STEER_DRIVER_FACTOR = 1 # from dbc + STEER_MAX = 300 # Max heading control assist torque 3.00 Nm + STEER_DRIVER_MULTIPLIER = 3 # weight driver torque heavily + STEER_DRIVER_FACTOR = 1 # from dbc + + STEER_TIME_MAX = 360 # Max time that EPS allows uninterrupted HCA steering control + STEER_TIME_ALERT = STEER_TIME_MAX - 10 # If mitigation fails, time to soft disengage before EPS timer expires + STEER_TIME_STUCK_TORQUE = 1.9 # EPS limits same torque to 6 seconds, reset timer 3x within that period + ACCEL_MAX = 2.0 # 2.0 m/s max acceleration + ACCEL_MIN = -3.5 # 3.5 m/s max deceleration + + def __init__(self, CP): can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) if CP.carFingerprint in PQ_CARS: