diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 97f00008bd..977818dbd5 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -86,27 +86,28 @@ class CarController(): # FIXME: this entire section is in desperate need of refactoring - if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: - if not enabled and CS.out.cruiseState.enabled: - # Cancel ACC if it's engaged with OP disengaged. - self.graButtonStatesToSend = BUTTON_STATES.copy() - self.graButtonStatesToSend["cancel"] = True - elif enabled and CS.out.standstill: - # Blip the Resume button if we're engaged at standstill. - # FIXME: This is a naive implementation, improve with visiond or radar input. - self.graButtonStatesToSend = BUTTON_STATES.copy() - self.graButtonStatesToSend["resumeCruise"] = True - - if CS.graMsgBusCounter != self.graMsgBusCounterPrev: - self.graMsgBusCounterPrev = CS.graMsgBusCounter - if self.graButtonStatesToSend is not None: - if self.graMsgSentCount == 0: - self.graMsgStartFramePrev = frame - idx = (CS.graMsgBusCounter + 1) % 16 - can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx)) - self.graMsgSentCount += 1 - if self.graMsgSentCount >= P.GRA_VBP_COUNT: - self.graButtonStatesToSend = None - self.graMsgSentCount = 0 + if CS.CP.pcmCruise: + if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: + if not enabled and CS.out.cruiseState.enabled: + # Cancel ACC if it's engaged with OP disengaged. + self.graButtonStatesToSend = BUTTON_STATES.copy() + self.graButtonStatesToSend["cancel"] = True + elif enabled and CS.esp_hold_confirmation: + # Blip the Resume button if we're engaged at standstill. + # FIXME: This is a naive implementation, improve with visiond or radar input. + self.graButtonStatesToSend = BUTTON_STATES.copy() + self.graButtonStatesToSend["resumeCruise"] = True + + if CS.graMsgBusCounter != self.graMsgBusCounterPrev: + self.graMsgBusCounterPrev = CS.graMsgBusCounter + if self.graButtonStatesToSend is not None: + if self.graMsgSentCount == 0: + self.graMsgStartFramePrev = frame + idx = (CS.graMsgBusCounter + 1) % 16 + can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx)) + self.graMsgSentCount += 1 + if self.graMsgSentCount >= P.GRA_VBP_COUNT: + self.graButtonStatesToSend = None + self.graMsgSentCount = 0 return can_sends diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index f02129a61c..6762996f9c 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -27,7 +27,7 @@ class CarState(CarStateBase): ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - ret.standstill = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]) + ret.standstill = ret.vEgo < 0.1 # Update steering angle, rate, yaw rate, and driver input torque. VW send # the sign/direction in a separate signal so they must be recombined. @@ -47,6 +47,7 @@ class CarState(CarStateBase): ret.gasPressed = ret.gas > 0 ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects ret.brakePressed = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"]) + self.esp_hold_confirmation = pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"] # Update gear and/or clutch position data. if trans_type == TransmissionType.automatic: @@ -94,13 +95,13 @@ class CarState(CarStateBase): ret.stockAeb = bool(ext_cp.vl["ACC_10"]["ANB_Teilbremsung_Freigabe"]) or bool(ext_cp.vl["ACC_10"]["ANB_Zielbremsung_Freigabe"]) # Update ACC radar status. - accStatus = pt_cp.vl["TSK_06"]["TSK_Status"] - if accStatus == 2: + self.tsk_status = pt_cp.vl["TSK_06"]["TSK_Status"] + if self.tsk_status == 2: # ACC okay and enabled, but not currently engaged ret.cruiseState.available = True ret.cruiseState.enabled = False - elif accStatus in [3, 4, 5]: - # ACC okay and enabled, currently engaged and regulating speed (3) or engaged with driver accelerating (4) or overrun (5) + elif self.tsk_status in [3, 4, 5]: + # ACC okay and enabled, currently regulating speed (3) or driver accel override (4) or overrun coast-down (5) ret.cruiseState.available = True ret.cruiseState.enabled = True else: diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index b8c9c5d67b..51b6a9a045 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -43,7 +43,7 @@ class CarInterface(CarInterfaceBase): else: ret.networkLocation = NetworkLocation.fwdCamera - # Global tuning defaults, can be overridden per-vehicle + # Global lateral tuning defaults, can be overridden per-vehicle ret.steerActuatorDelay = 0.05 ret.steerRateCost = 1.0 @@ -190,6 +190,8 @@ class CarInterface(CarInterfaceBase): # Vehicle health and operation safety checks if self.CS.parkingBrakeSet: events.add(EventName.parkBrake) + if self.CS.tsk_status in [6, 7]: + events.add(EventName.accFaulted) # Low speed steer alert hysteresis logic if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.):