diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 8eed1aa7ee..41444f2513 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, pqcan -from selfdrive.car.volkswagen.values import PQ_CARS, DBC_FILES, CANBUS, MQB_LDW_MESSAGES, PQ_LDW_MESSAGES, BUTTON_STATES, CarControllerParams as P +from selfdrive.car.volkswagen.values import PQ_CARS, DBC_FILES, CANBUS, MQB_LDW_MESSAGES, PQ_LDW_MESSAGES, CarControllerParams as P VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -22,11 +22,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): @@ -100,33 +95,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.cruiseControl.resume: - # Send Resume button when planner wants car to move - 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 - if self.CP.carFingerprint in PQ_CARS: - can_sends.append(pqcan.create_pq_acc_buttons_control(self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx)) - else: - 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.cruiseControl.resume: + 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 455f164f5d..294e929b2b 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -4,12 +4,12 @@ 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 PQ_CARS, DBC_FILES, CANBUS, NetworkLocation, TransmissionType, GearShifter, BUTTON_STATES, CarControllerParams - +from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, NetworkLocation, TransmissionType, GearShifter, \ + CarControllerParams, PQ_CARS, PQ_BUTTONS, MQB_BUTTONS class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) - self.buttonStates = BUTTON_STATES.copy() + self.button_states = {button.event_type: False for button in MQB_BUTTONS} if CP.carFingerprint in PQ_CARS: can_define = CANDefine(DBC_FILES.pq) @@ -124,26 +124,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 @@ -253,28 +247,20 @@ 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"]) or bool(pt_cp.vl["GRA_Neu"]["GRA_Up_lang"]) - self.buttonStates["decelCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Down_kurz"]) or bool(pt_cp.vl["GRA_Neu"]["GRA_Down_lang"]) - 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 = bool(pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"]) ret.rightBlinker = bool(pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"]) - - # 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_Neu"]["GRA_Hauptschalt"] - self.graSenderCoding = pt_cp.vl["GRA_Neu"]["GRA_Sender"] - self.graTypHauptschalter = False - self.graButtonTypeInfo = False - self.graTipStufe2 = False - - # 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_Neu"]["COUNTER"] + self.gra_stock_values = pt_cp.vl["GRA_Neu"] + buttonEvents = [] + for button in PQ_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 = bool(pt_cp.vl["Bremse_1"]["ESP_Passiv_getastet"]) @@ -325,6 +311,7 @@ class CarState(CarStateBase): ("GRA_Tip_Wiederaufnahme", "GRA_ACC_01"), # ACC button, resume ("GRA_Verstellung_Zeitluecke", "GRA_ACC_01"), # ACC button, time gap adj ("GRA_Typ_Hauptschalter", "GRA_ACC_01"), # ACC main button type + ("GRA_Codierung", "GRA_ACC_01"), # ACC button configuration/coding ("GRA_Tip_Stufe_2", "GRA_ACC_01"), # unknown related to stalk type ("GRA_ButtonTypeInfo", "GRA_ACC_01"), # unknown related to stalk type ("COUNTER", "GRA_ACC_01"), # GRA_ACC_01 CAN message counter @@ -435,6 +422,8 @@ class CarState(CarStateBase): ("GK1_Blinker_re", "Gate_Komf_1"), # Right turn signal on ("Bremsinfo", "Kombi_1"), # Manual handbrake applied ("GRA_Hauptschalt", "GRA_Neu"), # ACC button, on/off + ("GRA_Typ_Hauptschalt", "GRA_Neu"), # ACC button, momentary vs latching + ("GRA_Kodierinfo", "GRA_Neu"), # ACC button, configuration ("GRA_Abbrechen", "GRA_Neu"), # ACC button, cancel ("GRA_Neu_Setzen", "GRA_Neu"), # ACC button, set ("GRA_Up_lang", "GRA_Neu"), # ACC button, increase or accel, long press @@ -444,7 +433,7 @@ class CarState(CarStateBase): ("GRA_Recall", "GRA_Neu"), # ACC button, resume ("GRA_Zeitluecke", "GRA_Neu"), # ACC button, time gap adj ("COUNTER", "GRA_Neu"), # ACC button, message counter - ("GRA_Sender", "GRA_Neu"), # GRA Sender Coding + ("GRA_Sender", "GRA_Neu"), # ACC button, CAN message originator ] checks = [ diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 3bec4df209..cf62327ee0 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,6 +1,6 @@ from cereal import car from common.conversions import Conversions as CV -from selfdrive.car.volkswagen.values import CAR, PQ_CARS, BUTTON_STATES, CANBUS, NetworkLocation, TransmissionType, GearShifter +from selfdrive.car.volkswagen.values import CAR, PQ_CARS, 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 @@ -11,8 +11,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 @@ -182,20 +180,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 @@ -207,10 +194,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 8e568fc649..53073bfdd3 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: @@ -19,9 +20,6 @@ class CarControllerParams: PQ_LDW_STEP = 5 # LDW_1 message frequency 20Hz on PQ35/PQ46/NMS GRA_ACC_STEP = 3 # GRA_ACC_01/GRA_Neu message frequency 33Hz - GRA_VBP_STEP = 100 # Send ACC virtual button presses once a second - GRA_VBP_COUNT = 16 # Send VBP messages for ~0.5s (GRA_ACC_STEP * 16) - # Observed documented MQB limits: 3.00 Nm max, rate of change 5.00 Nm/sec. # Limiting rate-of-change based on real-world testing and Comma's safety # requirements for minimum time to lane departure. @@ -45,14 +43,27 @@ class DBC_FILES: # FIXME: PlotJuggler will assume wrong DBC for PQ, redo this and replace current handling of CANPacker/CANDefine 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.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]), +] + + +# TODO: for OP long, PQ may need special handling for the separate up/down short/long press signals +PQ_BUTTONS = [ + Button(car.CarState.ButtonEvent.Type.setCruise, "GRA_Neu", "GRA_Neu_Setzen", [1]), + Button(car.CarState.ButtonEvent.Type.resumeCruise, "GRA_Neu", "GRA_Recall", [1]), + Button(car.CarState.ButtonEvent.Type.accelCruise, "GRA_Neu", "GRA_Up_kurz", [1]), + Button(car.CarState.ButtonEvent.Type.decelCruise, "GRA_Neu", "GRA_Down_kurz", [1]), + Button(car.CarState.ButtonEvent.Type.cancel, "GRA_Neu", "GRA_Abbrechen", [1]), + Button(car.CarState.ButtonEvent.Type.gapAdjustCruise, "GRA_Neu", "GRA_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 0d8d04766b..1d8b694c9f 100644 --- a/selfdrive/car/volkswagen/volkswagencan.py +++ b/selfdrive/car/volkswagen/volkswagencan.py @@ -29,18 +29,10 @@ 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() + + values["GRA_Abbrechen"] = cancel + values["GRA_Tip_Wiederaufnahme"] = resume + return packer.make_can_msg("GRA_ACC_01", bus, values, idx)