From c20c3c5ecc0141d0b67afa3be338e16b35532a00 Mon Sep 17 00:00:00 2001 From: Jason Wen <47793918+sunnyhaibin@users.noreply.github.com> Date: Sat, 18 Feb 2023 17:58:56 -0500 Subject: [PATCH] MADS: Volkswagen control logic fix (#36) * use old method * can't miss this --- selfdrive/car/volkswagen/carstate.py | 22 +++++++++++++++++++++- selfdrive/car/volkswagen/interface.py | 22 ++++++++++++++++++++-- selfdrive/car/volkswagen/values.py | 10 ++++++++++ 3 files changed, 51 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index a0014fcf18..a9877d7667 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -4,7 +4,7 @@ from common.conversions import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from selfdrive.car.volkswagen.values import DBC, CANBUS, PQ_CARS, NetworkLocation, TransmissionType, GearShifter, \ - CarControllerParams + CarControllerParams, BUTTON_STATES class CarState(CarStateBase): @@ -14,6 +14,8 @@ class CarState(CarStateBase): self.button_states = {button.event_type: False for button in self.CCP.BUTTONS} self.esp_hold_confirmation = False self.upscale_lead_car_signal = False + self.buttonStates = BUTTON_STATES.copy() + self.buttonStatesPrev = BUTTON_STATES.copy() def create_button_events(self, pt_cp, buttons): button_events = [] @@ -36,6 +38,7 @@ class CarState(CarStateBase): ret = car.CarState.new_message() self.prev_mads_enabled = self.mads_enabled + self.buttonStatesPrev = self.buttonStates.copy() # Update vehicle speed and acceleration from ABS wheel speeds. ret.wheelSpeeds = self.get_wheel_speeds( @@ -137,6 +140,14 @@ class CarState(CarStateBase): if ret.cruiseState.speed > 90: ret.cruiseState.speed = 0 + # Update control button states for turn signals and ACC controls. + self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Hoch"]) + self.buttonStates["decelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Runter"]) + self.buttonStates["cancel"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Abbrechen"]) + self.buttonStates["setCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Setzen"]) + self.buttonStates["resumeCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Wiederaufnahme"]) + self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Verstellung_Zeitluecke"]) + # Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough ret.leftBlinker = ret.leftBlinkerOn = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Left"]) ret.rightBlinker = ret.rightBlinkerOn = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"]) @@ -156,6 +167,7 @@ class CarState(CarStateBase): ret = car.CarState.new_message() self.prev_mads_enabled = self.mads_enabled + self.buttonStatesPrev = self.buttonStates.copy() # Update vehicle speed and acceleration from ABS wheel speeds. ret.wheelSpeeds = self.get_wheel_speeds( @@ -247,6 +259,14 @@ class CarState(CarStateBase): if ret.cruiseState.speed > 70: # 255 kph in m/s == no current setpoint ret.cruiseState.speed = 0 + # Update control button states for turn signals and ACC controls. + self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Up_kurz"]) + self.buttonStates["decelCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Down_kurz"]) + self.buttonStates["cancel"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Abbrechen"]) + self.buttonStates["setCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Neu_Setzen"]) + self.buttonStates["resumeCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Recall"]) + self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Zeitluecke"]) + # Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough ret.leftBlinker, ret.rightBlinker = ret.leftBlinkerOn, ret.rightBlinkerOn = self.update_blinker_from_stalk(300, pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"], pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"]) diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 6872f77176..bd4c23cd70 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -3,7 +3,7 @@ from panda import Panda from common.conversions import Conversions as CV from selfdrive.car import STD_CARGO_KG, get_safety_config, create_mads_event from selfdrive.car.interfaces import CarInterfaceBase -from selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter +from selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter, BUTTON_STATES ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -20,6 +20,8 @@ class CarInterface(CarInterfaceBase): self.ext_bus = CANBUS.cam self.cp_ext = self.cp_cam + self.buttonStatesPrev = BUTTON_STATES.copy() + @staticmethod def _get_params(ret, candidate, fingerprint, car_fw, experimental_long): ret.carName = "volkswagen" @@ -222,10 +224,21 @@ class CarInterface(CarInterfaceBase): def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType) + buttonEvents = [] + + # Check for and process state-change events (button press or release) from + # the turn stalk switch or ACC steering wheel/control stalk buttons. + for button in self.CS.buttonStates: + if self.CS.buttonStates[button] != self.buttonStatesPrev[button]: + be = car.CarState.ButtonEvent.new_message() + be.type = button + be.pressed = self.CS.buttonStates[button] + buttonEvents.append(be) + self.CS.mads_enabled = False if not self.CS.control_initialized else ret.cruiseState.available self.CS.accEnabled, buttonEvents = self.get_sp_v_cruise_non_pcm_state(ret.cruiseState.available, self.CS.accEnabled, - ret.buttonEvents, c.vCruise, + buttonEvents, c.vCruise, enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise)) if ret.cruiseState.available: @@ -260,6 +273,8 @@ class CarInterface(CarInterfaceBase): buttonEvents.append(create_mads_event(self.mads_event_lock)) self.mads_event_lock = True + ret.buttonEvents = buttonEvents + events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic], pcm_enable=False, enable_buttons=(ButtonType.setCruise, ButtonType.resumeCruise)) @@ -283,6 +298,9 @@ class CarInterface(CarInterfaceBase): ret.events = events.to_msg() + # update previous car states + self.buttonStatesPrev = self.CS.buttonStates.copy() + return ret def apply(self, c, now_nanos): diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index cd30bef73b..fac13901df 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -103,6 +103,16 @@ class CANBUS: cam = 2 +BUTTON_STATES = { + "accelCruise": False, + "decelCruise": False, + "cancel": False, + "setCruise": False, + "resumeCruise": False, + "gapAdjustCruise": False +} + + # Check the 7th and 8th characters of the VIN before adding a new CAR. If the # chassis code is already listed below, don't add a new CAR, just add to the # FW_VERSIONS for that existing CAR.