sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

VW MQB: Cleanup stock ACC button handling (#25168)

Browse Source 
* 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 <shane@smiskol.com>
pull/25278/head
Jason Young 3 years ago committed by GitHub
parent 60eab24e0c
commit 0574b8504f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 44 additions and 95 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 32
      selfdrive/car/volkswagen/carcontroller.py
  2. 36
      selfdrive/car/volkswagen/carstate.py
  3. 19
      selfdrive/car/volkswagen/interface.py
  4. 24
      selfdrive/car/volkswagen/values.py
  5. 22
      selfdrive/car/volkswagen/volkswagencan.py
  6. 2
      selfdrive/test/process_replay/ref_commit

32
selfdrive/car/volkswagen/carcontroller.py
Unescape View File

@ -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 self.CP.pcmCruise and self.frame % P.GRA_ACC_STEP == 0:
idx = (CS.gra_stock_values["COUNTER"] + 1) % 16
if CC.cruiseControl.cancel:
# Cancel ACC if it's engaged with OP disengaged.
self.graButtonStatesToSend = BUTTON_STATES.copy()
self.graButtonStatesToSend["cancel"] = True
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:
# 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
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

36
selfdrive/car/volkswagen/carstate.py
Unescape View File

@ -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

19
selfdrive/car/volkswagen/interface.py
Unescape View File

@ -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

24
selfdrive/car/volkswagen/values.py
Unescape View File

@ -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

22
selfdrive/car/volkswagen/volkswagencan.py
Unescape View File

@ -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)

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
8f918a54cfefcaada91a7eca0c14ca4d4b6f11c8
dfacf4d807e04b855832218803b5c025d06569b8
Write Preview
Loading…
Cancel
Save