Merge branch 'mqb-button-refactor' into pq-upstream

pull/24768/head
Jason Young 3 years ago
commit 0465306408
  1. 40
      selfdrive/car/volkswagen/carcontroller.py
  2. 73
      selfdrive/car/volkswagen/carstate.py
  3. 19
      selfdrive/car/volkswagen/interface.py
  4. 35
      selfdrive/car/volkswagen/values.py
  5. 20
      selfdrive/car/volkswagen/volkswagencan.py

@ -2,7 +2,7 @@ from cereal import car
from opendbc.can.packer import CANPacker from opendbc.can.packer import CANPacker
from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.car.volkswagen import volkswagencan, pqcan 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 VisualAlert = car.CarControl.HUDControl.VisualAlert
@ -22,11 +22,6 @@ class CarController:
self.hcaSameTorqueCount = 0 self.hcaSameTorqueCount = 0
self.hcaEnabledFrameCount = 0 self.hcaEnabledFrameCount = 0
self.graButtonStatesToSend = None
self.graMsgSentCount = 0
self.graMsgStartFramePrev = 0
self.graMsgBusCounterPrev = 0
self.steer_rate_limited = False self.steer_rate_limited = False
def update(self, CC, CS, ext_bus): def update(self, CC, CS, ext_bus):
@ -100,33 +95,12 @@ class CarController:
# **** ACC Button Controls ********************************************** # # **** ACC Button Controls ********************************************** #
# FIXME: this entire section is in desperate need of refactoring if self.CP.pcmCruise and self.frame % P.GRA_ACC_STEP == 0:
idx = (CS.gra_stock_values["COUNTER"] + 1) % 16
if self.CP.pcmCruise: if CC.cruiseControl.cancel:
if self.frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, idx, cancel=True))
if CC.cruiseControl.cancel: elif CC.cruiseControl.resume:
# Cancel ACC if it's engaged with OP disengaged. can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, idx, resume=True))
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
new_actuators = actuators.copy() new_actuators = actuators.copy()
new_actuators.steer = self.apply_steer_last / P.STEER_MAX new_actuators.steer = self.apply_steer_last / P.STEER_MAX

@ -4,12 +4,12 @@ from common.conversions import Conversions as CV
from selfdrive.car.interfaces import CarStateBase from selfdrive.car.interfaces import CarStateBase
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from opendbc.can.can_define import CANDefine 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): class CarState(CarStateBase):
def __init__(self, CP): def __init__(self, CP):
super().__init__(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: if CP.carFingerprint in PQ_CARS:
can_define = CANDefine(DBC_FILES.pq) can_define = CANDefine(DBC_FILES.pq)
@ -124,26 +124,20 @@ class CarState(CarStateBase):
if ret.cruiseState.speed > 90: if ret.cruiseState.speed > 90:
ret.cruiseState.speed = 0 ret.cruiseState.speed = 0
# Update control button states for turn signals and ACC controls. # Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough
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"])
ret.leftBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Left"]) ret.leftBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Left"])
ret.rightBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"]) ret.rightBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"])
self.gra_stock_values = pt_cp.vl["GRA_ACC_01"]
# Read ACC hardware button type configuration info that has to pass thru buttonEvents = []
# to the radar. Ends up being different for steering wheel buttons vs for button in MQB_BUTTONS:
# third stalk type controls. state = (pt_cp.vl[button.can_addr][button.can_msg] in button.values)
self.graHauptschalter = pt_cp.vl["GRA_ACC_01"]["GRA_Hauptschalter"] if self.button_states[button.event_type] != state:
self.graTypHauptschalter = pt_cp.vl["GRA_ACC_01"]["GRA_Typ_Hauptschalter"] event = car.CarState.ButtonEvent.new_message()
self.graButtonTypeInfo = pt_cp.vl["GRA_ACC_01"]["GRA_ButtonTypeInfo"] event.type = button.event_type
self.graTipStufe2 = pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Stufe_2"] event.pressed = state
# Pick up the GRA_ACC_01 CAN message counter so we can sync to it for buttonEvents.append(event)
# later cruise-control button spamming. self.button_states[button.event_type] = state
self.graMsgBusCounter = pt_cp.vl["GRA_ACC_01"]["COUNTER"] ret.buttonEvents = buttonEvents
# Additional safety checks performed in CarInterface. # Additional safety checks performed in CarInterface.
ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0 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 if ret.cruiseState.speed > 70: # 255 kph in m/s == no current setpoint
ret.cruiseState.speed = 0 ret.cruiseState.speed = 0
# Update control button states for turn signals and ACC controls. # Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough
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"])
ret.leftBlinker = bool(pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"]) ret.leftBlinker = bool(pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"])
ret.rightBlinker = bool(pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"]) ret.rightBlinker = bool(pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"])
self.gra_stock_values = pt_cp.vl["GRA_Neu"]
# Read ACC hardware button type configuration info that has to pass thru buttonEvents = []
# to the radar. Ends up being different for steering wheel buttons vs for button in PQ_BUTTONS:
# third stalk type controls. state = (pt_cp.vl[button.can_addr][button.can_msg] in button.values)
self.graHauptschalter = pt_cp.vl["GRA_Neu"]["GRA_Hauptschalt"] if self.button_states[button.event_type] != state:
self.graSenderCoding = pt_cp.vl["GRA_Neu"]["GRA_Sender"] event = car.CarState.ButtonEvent.new_message()
self.graTypHauptschalter = False event.type = button.event_type
self.graButtonTypeInfo = False event.pressed = state
self.graTipStufe2 = False buttonEvents.append(event)
self.button_states[button.event_type] = state
# Pick up the GRA_ACC_01 CAN message counter so we can sync to it for ret.buttonEvents = buttonEvents
# later cruise-control button spamming.
self.graMsgBusCounter = pt_cp.vl["GRA_Neu"]["COUNTER"]
# Additional safety checks performed in CarInterface. # Additional safety checks performed in CarInterface.
ret.espDisabled = bool(pt_cp.vl["Bremse_1"]["ESP_Passiv_getastet"]) 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_Tip_Wiederaufnahme", "GRA_ACC_01"), # ACC button, resume
("GRA_Verstellung_Zeitluecke", "GRA_ACC_01"), # ACC button, time gap adj ("GRA_Verstellung_Zeitluecke", "GRA_ACC_01"), # ACC button, time gap adj
("GRA_Typ_Hauptschalter", "GRA_ACC_01"), # ACC main button type ("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_Tip_Stufe_2", "GRA_ACC_01"), # unknown related to stalk type
("GRA_ButtonTypeInfo", "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 ("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 ("GK1_Blinker_re", "Gate_Komf_1"), # Right turn signal on
("Bremsinfo", "Kombi_1"), # Manual handbrake applied ("Bremsinfo", "Kombi_1"), # Manual handbrake applied
("GRA_Hauptschalt", "GRA_Neu"), # ACC button, on/off ("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_Abbrechen", "GRA_Neu"), # ACC button, cancel
("GRA_Neu_Setzen", "GRA_Neu"), # ACC button, set ("GRA_Neu_Setzen", "GRA_Neu"), # ACC button, set
("GRA_Up_lang", "GRA_Neu"), # ACC button, increase or accel, long press ("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_Recall", "GRA_Neu"), # ACC button, resume
("GRA_Zeitluecke", "GRA_Neu"), # ACC button, time gap adj ("GRA_Zeitluecke", "GRA_Neu"), # ACC button, time gap adj
("COUNTER", "GRA_Neu"), # ACC button, message counter ("COUNTER", "GRA_Neu"), # ACC button, message counter
("GRA_Sender", "GRA_Neu"), # GRA Sender Coding ("GRA_Sender", "GRA_Neu"), # ACC button, CAN message originator
] ]
checks = [ checks = [

@ -1,6 +1,6 @@
from cereal import car from cereal import car
from common.conversions import Conversions as CV 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 import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.interfaces import CarInterfaceBase
@ -11,8 +11,6 @@ class CarInterface(CarInterfaceBase):
def __init__(self, CP, CarController, CarState): def __init__(self, CP, CarController, CarState):
super().__init__(CP, CarController, CarState) super().__init__(CP, CarController, CarState)
self.buttonStatesPrev = BUTTON_STATES.copy()
if CP.networkLocation == NetworkLocation.fwdCamera: if CP.networkLocation == NetworkLocation.fwdCamera:
self.ext_bus = CANBUS.pt self.ext_bus = CANBUS.pt
self.cp_ext = self.cp self.cp_ext = self.cp
@ -182,20 +180,9 @@ class CarInterface(CarInterfaceBase):
# returns a car.CarState # returns a car.CarState
def _update(self, c): def _update(self, c):
buttonEvents = []
ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType) 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 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]) events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic])
# Low speed steer alert hysteresis logic # Low speed steer alert hysteresis logic
@ -207,10 +194,6 @@ class CarInterface(CarInterfaceBase):
events.add(EventName.belowSteerSpeed) events.add(EventName.belowSteerSpeed)
ret.events = events.to_msg() ret.events = events.to_msg()
ret.buttonEvents = buttonEvents
# update previous car states
self.buttonStatesPrev = self.CS.buttonStates.copy()
return ret return ret

@ -1,4 +1,4 @@
from collections import defaultdict from collections import defaultdict, namedtuple
from dataclasses import dataclass from dataclasses import dataclass
from enum import Enum from enum import Enum
from typing import Dict, List, Union from typing import Dict, List, Union
@ -11,6 +11,7 @@ Ecu = car.CarParams.Ecu
NetworkLocation = car.CarParams.NetworkLocation NetworkLocation = car.CarParams.NetworkLocation
TransmissionType = car.CarParams.TransmissionType TransmissionType = car.CarParams.TransmissionType
GearShifter = car.CarState.GearShifter GearShifter = car.CarState.GearShifter
Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
class CarControllerParams: class CarControllerParams:
@ -19,9 +20,6 @@ class CarControllerParams:
PQ_LDW_STEP = 5 # LDW_1 message frequency 20Hz on PQ35/PQ46/NMS 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_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. # 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 # Limiting rate-of-change based on real-world testing and Comma's safety
# requirements for minimum time to lane departure. # 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 # 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)) DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict(DBC_FILES.mqb, None))
BUTTON_STATES = {
"accelCruise": False, MQB_BUTTONS = [
"decelCruise": False, Button(car.CarState.ButtonEvent.Type.setCruise, "GRA_ACC_01", "GRA_Tip_Setzen", [1]),
"cancel": False, Button(car.CarState.ButtonEvent.Type.resumeCruise, "GRA_ACC_01", "GRA_Tip_Wiederaufnahme", [1]),
"setCruise": False, Button(car.CarState.ButtonEvent.Type.accelCruise, "GRA_ACC_01", "GRA_Tip_Hoch", [1]),
"resumeCruise": False, Button(car.CarState.ButtonEvent.Type.decelCruise, "GRA_ACC_01", "GRA_Tip_Runter", [1]),
"gapAdjustCruise": False 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 = { MQB_LDW_MESSAGES = {
"none": 0, # Nothing to display "none": 0, # Nothing to display

@ -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) return packer.make_can_msg("LDW_02", bus, values)
def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx): def create_mqb_acc_buttons_control(packer, bus, gra_stock_values, idx, cancel=False, resume=False):
values = { values = gra_stock_values.copy()
"GRA_Hauptschalter": CS.graHauptschalter,
"GRA_Abbrechen": buttonStatesToSend["cancel"], values["GRA_Abbrechen"] = cancel
"GRA_Tip_Setzen": buttonStatesToSend["setCruise"], values["GRA_Tip_Wiederaufnahme"] = resume
"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
}
return packer.make_can_msg("GRA_ACC_01", bus, values, idx) return packer.make_can_msg("GRA_ACC_01", bus, values, idx)

Loading…
Cancel
Save