From 2c1184d7c8a9356e8e4da2a48f2f873224e03874 Mon Sep 17 00:00:00 2001 From: Jason Young <46612682+jyoung8607@users.noreply.github.com> Date: Mon, 25 Jul 2022 16:15:14 -0500 Subject: [PATCH] VW MQB: Cleanup stock ACC button handling (#25168) * VW MQB: Cleanup stock ACC button handling * use controls resume output as trigger * these can wait until taco bell * pass through of previously fixed value * retry CI * checks already done in carcontroller * don't need these anymore * reduce diff for now * remove parenthesis * update refs * fix packer exception * update refs Co-authored-by: Shane Smiskol old-commit-hash: 0574b8504f85132bfbe5d8a41913e37ad030401e --- selfdrive/car/volkswagen/carcontroller.py | 36 +++++------------------ selfdrive/car/volkswagen/carstate.py | 36 ++++++++++------------- selfdrive/car/volkswagen/interface.py | 19 +----------- selfdrive/car/volkswagen/values.py | 24 +++++++-------- selfdrive/car/volkswagen/volkswagencan.py | 22 +++++--------- selfdrive/test/process_replay/ref_commit | 2 +- 6 files changed, 44 insertions(+), 95 deletions(-) diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 5543d4f05c..99a3ce55f4 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,10 +17,6 @@ class CarController: self.hcaSameTorqueCount = 0 self.hcaEnabledFrameCount = 0 - self.graButtonStatesToSend = None - self.graMsgSentCount = 0 - self.graMsgStartFramePrev = 0 - self.graMsgBusCounterPrev = 0 def update(self, CC, CS, ext_bus): actuators = CC.actuators @@ -83,30 +79,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 - 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 9c4a111e4e..052a1262e4 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 @@ -181,6 +176,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 diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 225a6a32ca..8bab93d4a3 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,19 +158,8 @@ 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) - # 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 @@ -184,10 +171,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 80cd421d64..52a03c320c 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: @@ -18,9 +19,6 @@ class CarControllerParams: LDW_STEP = 10 # LDW_02 message frequency 10Hz GRA_ACC_STEP = 3 # GRA_ACC_01 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. @@ -43,14 +41,16 @@ 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.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 1b42d1a405..f4c21eeed4 100644 --- a/selfdrive/car/volkswagen/volkswagencan.py +++ b/selfdrive/car/volkswagen/volkswagencan.py @@ -32,19 +32,11 @@ 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 = { - "COUNTER": idx, - "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["COUNTER"] = idx + values["GRA_Abbrechen"] = cancel + values["GRA_Tip_Wiederaufnahme"] = resume + return packer.make_can_msg("GRA_ACC_01", bus, values) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index ce4f5c1711..0c1fa408e7 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -8f918a54cfefcaada91a7eca0c14ca4d4b6f11c8 \ No newline at end of file +dfacf4d807e04b855832218803b5c025d06569b8 \ No newline at end of file