sadmen
/
dragonpilot
mirror of https://github.com/dragonpilot-community/dragonpilot.git
1
0
Fork 0
Code Issues 67 Packages Projects Releases Wiki Activity

VW MQB: Clean up carcontroller (#21300)

Browse Source
pull/214/head
Jason Young 4 years ago committed by GitHub
parent 3901f28cb9
commit 3cb2b0eb7d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 15 additions and 88 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. 103
      selfdrive/car/volkswagen/carcontroller.py

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

@ -1,7 +1,7 @@
from cereal import car
from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.car.volkswagen import volkswagencan
from selfdrive.car.volkswagen.values import DBC, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams
from selfdrive.car.volkswagen.values import DBC, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams as P
from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
@ -24,64 +24,35 @@ class CarController():
def update(self, enabled, CS, frame, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart):
""" Controls thread """
P = CarControllerParams
# Send CAN commands.
can_sends = []
#--------------------------------------------------------------------------
# #
# Prepare HCA_01 Heading Control Assist messages with steering torque. #
# #
#--------------------------------------------------------------------------
# The factory camera sends at 50Hz while steering and 1Hz when not. When
# OP is active, Panda filters HCA_01 from the factory camera and OP emits
# HCA_01 at 50Hz. Rate switching creates some confusion in Cabana and
# doesn't seem to add value at this time. The rack will accept HCA_01 at
# 100Hz if we want to control at finer resolution in the future.
# **** Steering Controls ************************************************ #
if frame % P.HCA_STEP == 0:
# Logic to avoid HCA state 4 "refused":
# * Don't steer unless HCA is in state 3 "ready" or 5 "active"
# * Don't steer at standstill
# * Don't send > 3.00 Newton-meters torque
# * Don't send the same torque for > 6 seconds
# * Don't send uninterrupted steering for > 360 seconds
# One frame of HCA disabled is enough to reset the timer, without zeroing the
# torque value. Do that anytime we happen to have 0 torque, or failing that,
# when exceeding ~1/3 the 360 second timer.
# FAULT AVOIDANCE: HCA must not be enabled at standstill. Also stop
# commanding HCA if there's a fault, so the steering rack recovers.
if enabled and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning):
# FAULT AVOIDANCE: Requested HCA torque must not exceed 3.0 Nm. This
# is inherently handled by scaling to STEER_MAX. The rack doesn't seem
# to care about up/down rate, but we have some evidence it may do its
# own rate limiting, and matching OP helps for accurate tuning.
new_steer = int(round(actuators.steer * P.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
self.steer_rate_limited = new_steer != apply_steer
# FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending
# a single frame with HCA disabled is an effective workaround.
if apply_steer == 0:
# We can usually reset the timer for free, just by disabling HCA
# when apply_steer is exactly zero, which happens by chance during
# many steer torque direction changes. This could be expanded with
# a small dead-zone to capture all zero crossings, but not seeing a
# major need at this time.
hcaEnabled = False
self.hcaEnabledFrameCount = 0
else:
self.hcaEnabledFrameCount += 1
if self.hcaEnabledFrameCount >= 118 * (100 / P.HCA_STEP): # 118s
# The Kansas I-70 Crosswind Problem: if we truly do need to steer
# in one direction for > 360 seconds, we have to disable HCA for a
# frame while actively steering. Testing shows we can just set the
# disabled flag, and keep sending non-zero torque, which keeps the
# Panda torque rate limiting safety happy. Do so 3x within the 360
# second window for safety and redundancy.
hcaEnabled = False
self.hcaEnabledFrameCount = 0
else:
hcaEnabled = True
# FAULT AVOIDANCE: HCA torque must not be static for > 6 seconds.
# This is to detect the sending camera being stuck or frozen. OP
# can trip this on a curve if steering is saturated. Avoid this by
# reducing torque 0.01 Nm for one frame. Do so 3x within the 6
# second period for safety and redundancy.
if self.apply_steer_last == apply_steer:
self.hcaSameTorqueCount += 1
if self.hcaSameTorqueCount > 1.9 * (100 / P.HCA_STEP): # 1.9s
@ -89,9 +60,7 @@ class CarController():
self.hcaSameTorqueCount = 0
else:
self.hcaSameTorqueCount = 0
else:
# Continue sending HCA_01 messages, with the enable flags turned off.
hcaEnabled = False
apply_steer = 0
@ -100,15 +69,7 @@ class CarController():
can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_pt, CANBUS.pt, apply_steer,
idx, hcaEnabled))
#--------------------------------------------------------------------------
# #
# Prepare LDW_02 HUD messages with lane borders, confidence levels, and #
# the LKAS status LED. #
# #
#--------------------------------------------------------------------------
# The factory camera emits this message at 10Hz. When OP is active, Panda
# filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz.
# **** HUD Controls ***************************************************** #
if frame % P.LDW_STEP == 0:
if visual_alert in [VisualAlert.steerRequired, VisualAlert.ldw]:
@ -116,7 +77,6 @@ class CarController():
else:
hud_alert = MQB_LDW_MESSAGES["none"]
can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, enabled,
CS.out.steeringPressed, hud_alert, left_lane_visible,
right_lane_visible, CS.ldw_lane_warning_left,
@ -124,17 +84,9 @@ class CarController():
CS.ldw_dlc, CS.ldw_tlc, CS.out.standstill,
left_lane_depart, right_lane_depart))
#--------------------------------------------------------------------------
# #
# Prepare GRA_ACC_01 ACC control messages with button press events. #
# #
#--------------------------------------------------------------------------
# **** ACC Button Controls ********************************************** #
# The car sends this message at 33hz. OP sends it on-demand only for
# virtual button presses.
#
# First create any virtual button press event needed by openpilot, to sync
# stock ACC with OP disengagement, or to auto-resume from stop.
# FIXME: this entire section is in desperate need of refactoring
if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
if not enabled and CS.out.cruiseState.enabled:
@ -148,31 +100,6 @@ class CarController():
self.graButtonStatesToSend = BUTTON_STATES.copy()
self.graButtonStatesToSend["resumeCruise"] = True
# OP/Panda can see this message but can't filter it when integrated at the
# R242 LKAS camera. It could do so if integrated at the J533 gateway, but
# we need a generalized solution that works for either. The message is
# counter-protected, so we need to time our transmissions very precisely
# to achieve fast and fault-free switching between message flows accepted
# at the J428 ACC radar.
#
# Example message flow on the bus, frequency of 33Hz (GRA_ACC_STEP):
#
# CAR: 0 1 2 3 4 5 6 7 8 9 A B C D E F 0 1 2 3 4 5 6
# EON: 3 4 5 6 7 8 9 A B C D E F 0 1 2 GG^
#
# If OP needs to send a button press, it waits to see a GRA_ACC_01 message
# counter change, and then immediately follows up with the next increment.
# The OP message will be sent within about 1ms of the car's message, which
# is about 2ms before the car's next message is expected. OP sends for an
# arbitrary duration of 16 messages / ~0.5 sec, in lockstep with each new
# message from the car.
#
# Because OP's counter is synced to the car, J428 immediately accepts the
# OP messages as valid. Further messages from the car get discarded as
# duplicates without a fault. When OP stops sending, the extra time gap
# (GG) to the next valid car message is less than 1 * GRA_ACC_STEP. J428
# tolerates the gap just fine and control returns to the car immediately.
if CS.graMsgBusCounter != self.graMsgBusCounterPrev:
self.graMsgBusCounterPrev = CS.graMsgBusCounter
if self.graButtonStatesToSend is not None:

Write Preview
Loading…
Cancel
Save