diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 4614463c6e..565ded6e33 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -2,7 +2,7 @@ from cereal import car from opendbc.can.packer import CANPacker from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.volkswagen import volkswagencan -from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams as P +from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, MQB_LDW_MESSAGES, CarControllerParams as P VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -17,11 +17,6 @@ class CarController: self.hcaSameTorqueCount = 0 self.hcaEnabledFrameCount = 0 - self.graButtonStatesToSend = None - self.graMsgSentCount = 0 - self.graMsgStartFramePrev = 0 - self.graMsgBusCounterPrev = 0 - self.steer_rate_limited = False def update(self, CC, CS, ext_bus): @@ -88,31 +83,12 @@ class CarController: # **** ACC Button Controls ********************************************** # - # FIXME: this entire section is in desperate need of refactoring - - if self.CP.pcmCruise: - if self.frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: - if CC.cruiseControl.cancel: - # Cancel ACC if it's engaged with OP disengaged. - self.graButtonStatesToSend = BUTTON_STATES.copy() - self.graButtonStatesToSend["cancel"] = True - elif CC.enabled and CS.out.cruiseState.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 = self.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 self.CP.pcmCruise and self.frame % P.GRA_ACC_STEP == 0: + idx = (CS.gra_stock_values["COUNTER"] + 1) % 16 + if CC.cruiseControl.cancel: + can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, idx, cancel=True)) + elif CC.enabled and CS.out.cruiseState.standstill: + can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, idx, resume=True)) new_actuators = actuators.copy() new_actuators.steer = self.apply_steer_last / P.STEER_MAX diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 9c4a111e4e..67d773020b 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -4,18 +4,19 @@ from common.conversions import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine -from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, NetworkLocation, TransmissionType, GearShifter, BUTTON_STATES, CarControllerParams +from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, NetworkLocation, TransmissionType, GearShifter, \ + CarControllerParams, MQB_BUTTONS class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) + self.button_states = {button.event_type: False for button in MQB_BUTTONS} can_define = CANDefine(DBC_FILES.mqb) if CP.transmissionType == TransmissionType.automatic: self.shifter_values = can_define.dv["Getriebe_11"]["GE_Fahrstufe"] elif CP.transmissionType == TransmissionType.direct: self.shifter_values = can_define.dv["EV_Gearshift"]["GearPosition"] self.hca_status_values = can_define.dv["LH_EPS_03"]["EPS_HCA_Status"] - self.buttonStates = BUTTON_STATES.copy() def update(self, pt_cp, cam_cp, ext_cp, trans_type): ret = car.CarState.new_message() @@ -114,26 +115,20 @@ 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 = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Left"]) ret.rightBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"]) - - # Read ACC hardware button type configuration info that has to pass thru - # to the radar. Ends up being different for steering wheel buttons vs - # third stalk type controls. - self.graHauptschalter = pt_cp.vl["GRA_ACC_01"]["GRA_Hauptschalter"] - self.graTypHauptschalter = pt_cp.vl["GRA_ACC_01"]["GRA_Typ_Hauptschalter"] - self.graButtonTypeInfo = pt_cp.vl["GRA_ACC_01"]["GRA_ButtonTypeInfo"] - self.graTipStufe2 = pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Stufe_2"] - # Pick up the GRA_ACC_01 CAN message counter so we can sync to it for - # later cruise-control button spamming. - self.graMsgBusCounter = pt_cp.vl["GRA_ACC_01"]["COUNTER"] + self.gra_stock_values = pt_cp.vl["GRA_ACC_01"] + buttonEvents = [] + for button in MQB_BUTTONS: + state = (pt_cp.vl[button.can_addr][button.can_msg] in button.values) + if self.button_states[button.event_type] != state: + event = car.CarState.ButtonEvent.new_message() + event.type = button.event_type + event.pressed = state + buttonEvents.append(event) + self.button_states[button.event_type] = state + ret.buttonEvents = buttonEvents # Additional safety checks performed in CarInterface. ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0 @@ -159,8 +154,10 @@ class CarState(CarStateBase): ("ZV_HFS_offen", "Gateway_72"), # Door open, rear left ("ZV_HBFS_offen", "Gateway_72"), # Door open, rear right ("ZV_HD_offen", "Gateway_72"), # Trunk or hatch open - ("Comfort_Signal_Left", "Blinkmodi_02"), # Left turn signal including comfort blink interval - ("Comfort_Signal_Right", "Blinkmodi_02"), # Right turn signal including comfort blink interval + ("BH_Blinker_li", "Gateway_72"), # Left turn signal, instantaneous state of turn stalk + ("BH_Blinker_re", "Gateway_72"), # Right turn signal, instantaneous state of turn stalk + ("Comfort_Signal_Left", "Blinkmodi_02"), # Left turn signal, active for duration of comfort signaling + ("Comfort_Signal_Right", "Blinkmodi_02"), # Right turn signal, active for duration of comfort signaling ("AB_Gurtschloss_FA", "Airbag_02"), # Seatbelt status, driver ("AB_Gurtschloss_BF", "Airbag_02"), # Seatbelt status, passenger ("ESP_Fahrer_bremst", "ESP_05"), # Brake pedal pressed diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index c2b077b6db..75f6e69530 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,5 +1,5 @@ from cereal import car -from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES, CANBUS, NetworkLocation, TransmissionType, GearShifter +from selfdrive.car.volkswagen.values import CAR, CANBUS, NetworkLocation, TransmissionType, GearShifter from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase @@ -10,8 +10,6 @@ class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController, CarState): super().__init__(CP, CarController, CarState) - self.buttonStatesPrev = BUTTON_STATES.copy() - if CP.networkLocation == NetworkLocation.fwdCamera: self.ext_bus = CANBUS.pt self.cp_ext = self.cp @@ -160,20 +158,9 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def _update(self, c): - buttonEvents = [] - ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType) ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - # 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) - events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic]) # Low speed steer alert hysteresis logic @@ -185,10 +172,6 @@ class CarInterface(CarInterfaceBase): events.add(EventName.belowSteerSpeed) ret.events = events.to_msg() - ret.buttonEvents = buttonEvents - - # update previous car states - self.buttonStatesPrev = self.CS.buttonStates.copy() return ret diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 6e64f705b0..3c03c30508 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -1,4 +1,4 @@ -from collections import defaultdict +from collections import defaultdict, namedtuple from dataclasses import dataclass from enum import Enum from typing import Dict, List, Union @@ -11,6 +11,7 @@ Ecu = car.CarParams.Ecu NetworkLocation = car.CarParams.NetworkLocation TransmissionType = car.CarParams.TransmissionType GearShifter = car.CarState.GearShifter +Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) class CarControllerParams: @@ -43,14 +44,18 @@ class DBC_FILES: DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict(DBC_FILES.mqb, None)) -BUTTON_STATES = { - "accelCruise": False, - "decelCruise": False, - "cancel": False, - "setCruise": False, - "resumeCruise": False, - "gapAdjustCruise": False -} + +MQB_BUTTONS = [ + Button(car.CarState.ButtonEvent.Type.leftBlinker, "Gateway_72", "BH_Blinker_li", [1]), + Button(car.CarState.ButtonEvent.Type.rightBlinker, "Gateway_72", "BH_Blinker_re", [1]), + Button(car.CarState.ButtonEvent.Type.setCruise, "GRA_ACC_01", "GRA_Tip_Setzen", [1]), + Button(car.CarState.ButtonEvent.Type.resumeCruise, "GRA_ACC_01", "GRA_Tip_Wiederaufnahme", [1]), + Button(car.CarState.ButtonEvent.Type.accelCruise, "GRA_ACC_01", "GRA_Tip_Hoch", [1]), + Button(car.CarState.ButtonEvent.Type.decelCruise, "GRA_ACC_01", "GRA_Tip_Runter", [1]), + Button(car.CarState.ButtonEvent.Type.cancel, "GRA_ACC_01", "GRA_Abbrechen", [1]), + Button(car.CarState.ButtonEvent.Type.gapAdjustCruise, "GRA_ACC_01", "GRA_Verstellung_Zeitluecke", [1]), +] + MQB_LDW_MESSAGES = { "none": 0, # Nothing to display diff --git a/selfdrive/car/volkswagen/volkswagencan.py b/selfdrive/car/volkswagen/volkswagencan.py index 10e0054c79..3850658c7e 100644 --- a/selfdrive/car/volkswagen/volkswagencan.py +++ b/selfdrive/car/volkswagen/volkswagencan.py @@ -32,18 +32,12 @@ def create_mqb_hud_control(packer, bus, enabled, steering_pressed, hud_alert, le }) return packer.make_can_msg("LDW_02", bus, values) -def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx): - values = { - "GRA_Hauptschalter": CS.graHauptschalter, - "GRA_Abbrechen": buttonStatesToSend["cancel"], - "GRA_Tip_Setzen": buttonStatesToSend["setCruise"], - "GRA_Tip_Hoch": buttonStatesToSend["accelCruise"], - "GRA_Tip_Runter": buttonStatesToSend["decelCruise"], - "GRA_Tip_Wiederaufnahme": buttonStatesToSend["resumeCruise"], - "GRA_Verstellung_Zeitluecke": 3 if buttonStatesToSend["gapAdjustCruise"] else 0, - "GRA_Typ_Hauptschalter": CS.graTypHauptschalter, - "GRA_Codierung": 2, - "GRA_Tip_Stufe_2": CS.graTipStufe2, - "GRA_ButtonTypeInfo": CS.graButtonTypeInfo - } +def create_mqb_acc_buttons_control(packer, bus, gra_stock_values, idx, cancel=False, resume=False): + values = gra_stock_values.copy() + + if cancel: + values["GRA_Abbrechen"] = True + elif resume: + values["GRA_Tip_Wiederaufnahme"] = True + return packer.make_can_msg("GRA_ACC_01", bus, values, idx)