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

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

Browse Source
pull/24768/head
Jason Young 3 years ago
parent 720c19db98 93f868ee23
commit 0465306408
5 changed files with 68 additions and 119 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. 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

40
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, 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

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

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

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

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

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

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

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

Write Preview
Loading…
Cancel
Save