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

Browse Source
pull/25168/head
Jason Young 3 years ago
parent 0c95493dc0
commit 13f17f71a8
5 changed files with 49 additions and 94 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. 38
      selfdrive/car/volkswagen/carcontroller.py
  2. 41
      selfdrive/car/volkswagen/carstate.py
  3. 19
      selfdrive/car/volkswagen/interface.py
  4. 23
      selfdrive/car/volkswagen/values.py
  5. 22
      selfdrive/car/volkswagen/volkswagencan.py

38
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,11 +17,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):
@ -88,31 +83,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.enabled and CS.out.cruiseState.standstill:
# Blip the Resume button if we're engaged at standstill.
# FIXME: This is a naive implementation, improve with visiond or radar input.
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.enabled and CS.out.cruiseState.standstill:
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

41
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
@ -159,8 +154,10 @@ class CarState(CarStateBase):
("ZV_HFS_offen", "Gateway_72"), # Door open, rear left
("ZV_HBFS_offen", "Gateway_72"), # Door open, rear right
("ZV_HD_offen", "Gateway_72"), # Trunk or hatch open
("Comfort_Signal_Left", "Blinkmodi_02"), # Left turn signal including comfort blink interval
("Comfort_Signal_Right", "Blinkmodi_02"), # Right turn signal including comfort blink interval
("BH_Blinker_li", "Gateway_72"), # Left turn signal, instantaneous state of turn stalk
("BH_Blinker_re", "Gateway_72"), # Right turn signal, instantaneous state of turn stalk
("Comfort_Signal_Left", "Blinkmodi_02"), # Left turn signal, active for duration of comfort signaling
("Comfort_Signal_Right", "Blinkmodi_02"), # Right turn signal, active for duration of comfort signaling
("AB_Gurtschloss_FA", "Airbag_02"), # Seatbelt status, driver
("AB_Gurtschloss_BF", "Airbag_02"), # Seatbelt status, passenger
("ESP_Fahrer_bremst", "ESP_05"), # Brake pedal pressed

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,20 +158,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
@ -185,10 +172,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

23
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:
@ -43,14 +44,18 @@ 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.leftBlinker, "Gateway_72", "BH_Blinker_li", [1]),
Button(car.CarState.ButtonEvent.Type.rightBlinker, "Gateway_72", "BH_Blinker_re", [1]),
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,18 +32,12 @@ 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()
if cancel:
values["GRA_Abbrechen"] = True
elif resume:
values["GRA_Tip_Wiederaufnahme"] = True
return packer.make_can_msg("GRA_ACC_01", bus, values, idx)

Write Preview
Loading…
Cancel
Save