mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-27 17:02:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

VW: Prep for MLB, EPS timer cleanup (#27624)

Browse Source 
* VW: Cleanup EPS timer handling, add Event

* add event

* revert that

* bump cereal

* remove accFaultedTemp
This commit is contained in:
Jason Young
2023-06-25 00:26:51 -04:00
committed by GitHub
parent 76ce71c6d6
commit 336a9ed54a
3 changed files with 43 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/volkswagen/carcontroller.py
+20 -24
View File
@@ -21,8 +21,9 @@ class CarController:
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 @@ class CarController:
# * 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 @@ class CarController:
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
selfdrive/car/volkswagen/interface.py
+7 -1
View File
@@ -20,6 +20,8 @@ class CarInterface(CarInterfaceBase):
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 @@ class CarInterface(CarInterfaceBase):
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
selfdrive/car/volkswagen/values.py
+16 -11
View File
@@ -19,20 +19,25 @@ Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
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.
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):
# 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
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
if CP.carFingerprint in PQ_CARS: