diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 48e9ed37d6..9107d41eb3 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -10,6 +10,8 @@ from openpilot.selfdrive.car.volkswagen.values import DBC, CANBUS, NetworkLocati class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) + self.frame = 0 + self.eps_init_complete = False self.CCP = CarControllerParams(CP) self.button_states = {button.event_type: False for button in self.CCP.BUTTONS} self.esp_hold_confirmation = False @@ -47,18 +49,14 @@ class CarState(CarStateBase): ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = ret.vEgoRaw == 0 - # Update steering angle, rate, yaw rate, and driver input torque. VW send - # the sign/direction in a separate signal so they must be recombined. + # Update EPS position and state info. For signed values, VW sends the sign in a separate signal. ret.steeringAngleDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradwinkel"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradwinkel"])] ret.steeringRateDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradw_Geschw"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradw_Geschw"])] ret.steeringTorque = pt_cp.vl["LH_EPS_03"]["EPS_Lenkmoment"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_Lenkmoment"])] ret.steeringPressed = abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE ret.yawRate = pt_cp.vl["ESP_02"]["ESP_Gierrate"] * (1, -1)[int(pt_cp.vl["ESP_02"]["ESP_VZ_Gierrate"])] * CV.DEG_TO_RAD - - # Verify EPS readiness to accept steering commands hca_status = self.CCP.hca_status_values.get(pt_cp.vl["LH_EPS_03"]["EPS_HCA_Status"]) - ret.steerFaultPermanent = hca_status in ("DISABLED", "FAULT") - ret.steerFaultTemporary = hca_status in ("INITIALIZING", "REJECTED") + ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status) # VW Emergency Assist status tracking and mitigation self.eps_stock_values = pt_cp.vl["LH_EPS_03"] @@ -151,6 +149,7 @@ class CarState(CarStateBase): # Digital instrument clusters expect the ACC HUD lead car distance to be scaled differently self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) + self.frame += 1 return ret def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type): @@ -168,18 +167,14 @@ class CarState(CarStateBase): ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = ret.vEgoRaw == 0 - # Update steering angle, rate, yaw rate, and driver input torque. VW send - # the sign/direction in a separate signal so they must be recombined. + # Update EPS position and state info. For signed values, VW sends the sign in a separate signal. ret.steeringAngleDeg = pt_cp.vl["Lenkhilfe_3"]["LH3_BLW"] * (1, -1)[int(pt_cp.vl["Lenkhilfe_3"]["LH3_BLWSign"])] ret.steeringRateDeg = pt_cp.vl["Lenkwinkel_1"]["Lenkradwinkel_Geschwindigkeit"] * (1, -1)[int(pt_cp.vl["Lenkwinkel_1"]["Lenkradwinkel_Geschwindigkeit_S"])] ret.steeringTorque = pt_cp.vl["Lenkhilfe_3"]["LH3_LM"] * (1, -1)[int(pt_cp.vl["Lenkhilfe_3"]["LH3_LMSign"])] ret.steeringPressed = abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE ret.yawRate = pt_cp.vl["Bremse_5"]["Giergeschwindigkeit"] * (1, -1)[int(pt_cp.vl["Bremse_5"]["Vorzeichen_der_Giergeschwindigk"])] * CV.DEG_TO_RAD - - # Verify EPS readiness to accept steering commands hca_status = self.CCP.hca_status_values.get(pt_cp.vl["Lenkhilfe_2"]["LH2_Sta_HCA"]) - ret.steerFaultPermanent = hca_status in ("DISABLED", "FAULT") - ret.steerFaultTemporary = hca_status in ("INITIALIZING", "REJECTED") + ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status) # Update gas, brakes, and gearshift. ret.gas = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] / 100.0 @@ -253,8 +248,17 @@ class CarState(CarStateBase): # Additional safety checks performed in CarInterface. ret.espDisabled = bool(pt_cp.vl["Bremse_1"]["ESP_Passiv_getastet"]) + self.frame += 1 return ret + def update_hca_state(self, hca_status): + # Treat INITIALIZING and FAULT as temporary for worst likely EPS recovery time, for cars without factory Lane Assist + # DISABLED means the EPS hasn't been configured to support Lane Assist + self.eps_init_complete = self.eps_init_complete or (hca_status in ("DISABLED", "READY", "ACTIVE") or self.frame > 600) + perm_fault = hca_status == "DISABLED" or (self.eps_init_complete and hca_status in ("INITIALIZING", "FAULT")) + temp_fault = hca_status == "REJECTED" or not self.eps_init_complete + return temp_fault, perm_fault + @staticmethod def get_can_parser(CP): if CP.flags & VolkswagenFlags.PQ: