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
Unified 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 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 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 VisualAlert = car.CarControl.HUDControl.VisualAlert
@ -17,11 +17,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):
@ -88,31 +83,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.enabled and CS.out.cruiseState.standstill:
# 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.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
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

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 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 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): class CarState(CarStateBase):
def __init__(self, CP): def __init__(self, CP):
super().__init__(CP) super().__init__(CP)
self.button_states = {button.event_type: False for button in MQB_BUTTONS}
can_define = CANDefine(DBC_FILES.mqb) can_define = CANDefine(DBC_FILES.mqb)
if CP.transmissionType == TransmissionType.automatic: if CP.transmissionType == TransmissionType.automatic:
self.shifter_values = can_define.dv["Getriebe_11"]["GE_Fahrstufe"] self.shifter_values = can_define.dv["Getriebe_11"]["GE_Fahrstufe"]
elif CP.transmissionType == TransmissionType.direct: elif CP.transmissionType == TransmissionType.direct:
self.shifter_values = can_define.dv["EV_Gearshift"]["GearPosition"] self.shifter_values = can_define.dv["EV_Gearshift"]["GearPosition"]
self.hca_status_values = can_define.dv["LH_EPS_03"]["EPS_HCA_Status"] 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): def update(self, pt_cp, cam_cp, ext_cp, trans_type):
ret = car.CarState.new_message() ret = car.CarState.new_message()
@ -114,26 +115,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
@ -159,8 +154,10 @@ class CarState(CarStateBase):
("ZV_HFS_offen", "Gateway_72"), # Door open, rear left ("ZV_HFS_offen", "Gateway_72"), # Door open, rear left
("ZV_HBFS_offen", "Gateway_72"), # Door open, rear right ("ZV_HBFS_offen", "Gateway_72"), # Door open, rear right
("ZV_HD_offen", "Gateway_72"), # Trunk or hatch open ("ZV_HD_offen", "Gateway_72"), # Trunk or hatch open
("Comfort_Signal_Left", "Blinkmodi_02"), # Left turn signal including comfort blink interval ("BH_Blinker_li", "Gateway_72"), # Left turn signal, instantaneous state of turn stalk
("Comfort_Signal_Right", "Blinkmodi_02"), # Right turn signal including comfort blink interval ("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_FA", "Airbag_02"), # Seatbelt status, driver
("AB_Gurtschloss_BF", "Airbag_02"), # Seatbelt status, passenger ("AB_Gurtschloss_BF", "Airbag_02"), # Seatbelt status, passenger
("ESP_Fahrer_bremst", "ESP_05"), # Brake pedal pressed ("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 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 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
@ -10,8 +10,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
@ -160,20 +158,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
@ -185,10 +172,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

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 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:
@ -43,14 +44,18 @@ class DBC_FILES:
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.leftBlinker, "Gateway_72", "BH_Blinker_li", [1]),
"cancel": False, Button(car.CarState.ButtonEvent.Type.rightBlinker, "Gateway_72", "BH_Blinker_re", [1]),
"setCruise": False, Button(car.CarState.ButtonEvent.Type.setCruise, "GRA_ACC_01", "GRA_Tip_Setzen", [1]),
"resumeCruise": False, Button(car.CarState.ButtonEvent.Type.resumeCruise, "GRA_ACC_01", "GRA_Tip_Wiederaufnahme", [1]),
"gapAdjustCruise": False 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 = { MQB_LDW_MESSAGES = {
"none": 0, # Nothing to display "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) 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"], if cancel:
"GRA_Tip_Setzen": buttonStatesToSend["setCruise"], values["GRA_Abbrechen"] = True
"GRA_Tip_Hoch": buttonStatesToSend["accelCruise"], elif resume:
"GRA_Tip_Runter": buttonStatesToSend["decelCruise"], values["GRA_Tip_Wiederaufnahme"] = True
"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)

Write Preview
Loading…
Cancel
Save