mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-08-03 08:41:47 +08:00
Clean up CarControllers (#25008)
* do VW * Do the rest * unused * ford cc formatting * final clean ups * also just return update output
This commit is contained in:
Shane Smiskol
@@ -1,8 +1,8 @@
|
||||
import numpy as np
|
||||
|
||||
from common.realtime import DT_CTRL
|
||||
from selfdrive.car.body import bodycan
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car.body import bodycan
|
||||
from selfdrive.car.body.values import SPEED_FROM_RPM
|
||||
from selfdrive.controls.lib.pid import PIDController
|
||||
|
||||
@@ -14,7 +14,7 @@ MAX_POS_INTEGRATOR = 0.2 # meters
|
||||
MAX_TURN_INTEGRATOR = 0.1 # meters
|
||||
|
||||
|
||||
class CarController():
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.frame = 0
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
from cereal import car
|
||||
from common.numpy_fast import clip, interp
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car.ford import fordcan
|
||||
from selfdrive.car.ford.values import CarControllerParams
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
@@ -17,11 +17,12 @@ def apply_ford_steer_angle_limits(apply_steer, apply_steer_last, vEgo):
|
||||
return apply_steer
|
||||
|
||||
|
||||
class CarController():
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.VM = VM
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.frame = 0
|
||||
|
||||
self.apply_steer_last = 0
|
||||
self.steer_rate_limited = False
|
||||
@@ -29,7 +30,7 @@ class CarController():
|
||||
self.lkas_enabled_last = False
|
||||
self.steer_alert_last = False
|
||||
|
||||
def update(self, CC, CS, frame):
|
||||
def update(self, CC, CS):
|
||||
can_sends = []
|
||||
|
||||
actuators = CC.actuators
|
||||
@@ -48,7 +49,7 @@ class CarController():
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
# send steering commands at 20Hz
|
||||
if (frame % CarControllerParams.LKAS_STEER_STEP) == 0:
|
||||
if (self.frame % CarControllerParams.LKAS_STEER_STEP) == 0:
|
||||
lca_rq = 1 if CC.latActive else 0
|
||||
|
||||
# use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented
|
||||
@@ -69,15 +70,14 @@ class CarController():
|
||||
can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision,
|
||||
path_offset, path_angle, curvature_rate, curvature))
|
||||
|
||||
|
||||
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
|
||||
|
||||
# send lkas ui command at 1Hz or if ui state changes
|
||||
if (frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
|
||||
if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
|
||||
can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values))
|
||||
|
||||
# send acc ui command at 20Hz or if ui state changes
|
||||
if (frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
|
||||
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
|
||||
can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, CS.acc_tja_status_stock_values))
|
||||
|
||||
self.main_on_last = main_on
|
||||
@@ -87,4 +87,5 @@ class CarController():
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steeringAngleDeg = apply_steer
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
@@ -78,6 +78,4 @@ class CarInterface(CarInterfaceBase):
|
||||
return ret
|
||||
|
||||
def apply(self, c):
|
||||
ret = self.CC.update(c, self.CS, self.frame)
|
||||
self.frame += 1
|
||||
return ret
|
||||
return self.CC.update(c, self.CS)
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
from cereal import car
|
||||
from common.conversions import Conversions as CV
|
||||
from common.realtime import DT_CTRL
|
||||
from common.numpy_fast import interp
|
||||
from common.realtime import DT_CTRL
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.gm import gmcan
|
||||
|
||||
@@ -184,5 +184,4 @@ class CarInterface(CarInterfaceBase):
|
||||
return ret
|
||||
|
||||
def apply(self, c):
|
||||
ret = self.CC.update(c, self.CS)
|
||||
return ret
|
||||
return self.CC.update(c, self.CS)
|
||||
|
||||
@@ -1,11 +1,11 @@
|
||||
from cereal import car
|
||||
from common.realtime import DT_CTRL
|
||||
from common.numpy_fast import clip, interp
|
||||
from common.conversions import Conversions as CV
|
||||
from common.numpy_fast import clip, interp
|
||||
from common.realtime import DT_CTRL
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.hyundai import hda2can, hyundaican
|
||||
from selfdrive.car.hyundai.values import Buttons, CarControllerParams, HDA2_CAR, CAR
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
|
||||
@@ -349,5 +349,4 @@ class CarInterface(CarInterfaceBase):
|
||||
return ret
|
||||
|
||||
def apply(self, c):
|
||||
ret = self.CC.update(c, self.CS)
|
||||
return ret
|
||||
return self.CC.update(c, self.CS)
|
||||
|
||||
@@ -1,46 +1,48 @@
|
||||
from cereal import car
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.mazda import mazdacan
|
||||
from selfdrive.car.mazda.values import CarControllerParams, Buttons
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
class CarController():
|
||||
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.apply_steer_last = 0
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.steer_rate_limited = False
|
||||
self.brake_counter = 0
|
||||
self.frame = 0
|
||||
|
||||
def update(self, c, CS, frame):
|
||||
def update(self, CC, CS):
|
||||
can_sends = []
|
||||
|
||||
apply_steer = 0
|
||||
self.steer_rate_limited = False
|
||||
|
||||
if c.latActive:
|
||||
if CC.latActive:
|
||||
# calculate steer and also set limits due to driver torque
|
||||
new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX))
|
||||
new_steer = int(round(CC.actuators.steer * CarControllerParams.STEER_MAX))
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
|
||||
CS.out.steeringTorque, CarControllerParams)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
if c.enabled:
|
||||
if CS.out.standstill and frame % 5 == 0:
|
||||
if CC.enabled:
|
||||
if CS.out.standstill and self.frame % 5 == 0:
|
||||
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
|
||||
# Send Resume button at 20hz if we're engaged at standstill to support full stop and go!
|
||||
# TODO: improve the resume trigger logic by looking at actual radar data
|
||||
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
|
||||
|
||||
if c.cruiseControl.cancel:
|
||||
if CC.cruiseControl.cancel:
|
||||
# If brake is pressed, let us wait >70ms before trying to disable crz to avoid
|
||||
# a race condition with the stock system, where the second cancel from openpilot
|
||||
# will disable the crz 'main on'. crz ctrl msg runs at 50hz. 70ms allows us to
|
||||
# read 3 messages and most likely sync state before we attempt cancel.
|
||||
self.brake_counter = self.brake_counter + 1
|
||||
if frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
|
||||
if self.frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
|
||||
# Cancel Stock ACC if it's enabled while OP is disengaged
|
||||
# Send at a rate of 10hz until we sync with stock ACC state
|
||||
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
|
||||
@@ -50,18 +52,19 @@ class CarController():
|
||||
self.apply_steer_last = apply_steer
|
||||
|
||||
# send HUD alerts
|
||||
if frame % 50 == 0:
|
||||
ldw = c.hudControl.visualAlert == VisualAlert.ldw
|
||||
steer_required = c.hudControl.visualAlert == VisualAlert.steerRequired
|
||||
if self.frame % 50 == 0:
|
||||
ldw = CC.hudControl.visualAlert == VisualAlert.ldw
|
||||
steer_required = CC.hudControl.visualAlert == VisualAlert.steerRequired
|
||||
# TODO: find a way to silence audible warnings so we can add more hud alerts
|
||||
steer_required = steer_required and CS.lkas_allowed_speed
|
||||
can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
|
||||
|
||||
# send steering command
|
||||
can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint,
|
||||
frame, apply_steer, CS.cam_lkas))
|
||||
self.frame, apply_steer, CS.cam_lkas))
|
||||
|
||||
new_actuators = c.actuators.copy()
|
||||
new_actuators = CC.actuators.copy()
|
||||
new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
@@ -90,6 +90,4 @@ class CarInterface(CarInterfaceBase):
|
||||
return ret
|
||||
|
||||
def apply(self, c):
|
||||
ret = self.CC.update(c, self.CS, self.frame)
|
||||
self.frame += 1
|
||||
return ret
|
||||
return self.CC.update(c, self.CS)
|
||||
|
||||
@@ -1,25 +1,27 @@
|
||||
from cereal import car
|
||||
from common.numpy_fast import clip, interp
|
||||
from selfdrive.car.nissan import nissancan
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car.nissan import nissancan
|
||||
from selfdrive.car.nissan.values import CAR, CarControllerParams
|
||||
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
class CarController():
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.frame = 0
|
||||
|
||||
self.lkas_max_torque = 0
|
||||
self.last_angle = 0
|
||||
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
def update(self, c, CS, frame, actuators, cruise_cancel, hud_alert,
|
||||
left_line, right_line, left_lane_depart, right_lane_depart):
|
||||
def update(self, CC, CS):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
|
||||
can_sends = []
|
||||
|
||||
@@ -28,9 +30,9 @@ class CarController():
|
||||
lkas_hud_info_msg = CS.lkas_hud_info_msg
|
||||
apply_angle = actuators.steeringAngleDeg
|
||||
|
||||
steer_hud_alert = 1 if hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
|
||||
steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
|
||||
|
||||
if c.latActive:
|
||||
if CC.latActive:
|
||||
# # windup slower
|
||||
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
|
||||
angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V)
|
||||
@@ -57,25 +59,25 @@ class CarController():
|
||||
|
||||
self.last_angle = apply_angle
|
||||
|
||||
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and cruise_cancel:
|
||||
can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg, frame))
|
||||
if self.CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA) and pcm_cancel_cmd:
|
||||
can_sends.append(nissancan.create_acc_cancel_cmd(self.packer, self.car_fingerprint, CS.cruise_throttle_msg, self.frame))
|
||||
|
||||
# TODO: Find better way to cancel!
|
||||
# For some reason spamming the cancel button is unreliable on the Leaf
|
||||
# We now cancel by making propilot think the seatbelt is unlatched,
|
||||
# this generates a beep and a warning message every time you disengage
|
||||
if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC) and frame % 2 == 0:
|
||||
can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, cruise_cancel))
|
||||
if self.CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC) and self.frame % 2 == 0:
|
||||
can_sends.append(nissancan.create_cancel_msg(self.packer, CS.cancel_msg, pcm_cancel_cmd))
|
||||
|
||||
can_sends.append(nissancan.create_steering_control(
|
||||
self.packer, apply_angle, frame, c.enabled, self.lkas_max_torque))
|
||||
self.packer, apply_angle, self.frame, CC.enabled, self.lkas_max_torque))
|
||||
|
||||
if lkas_hud_msg and lkas_hud_info_msg:
|
||||
if frame % 2 == 0:
|
||||
if self.frame % 2 == 0:
|
||||
can_sends.append(nissancan.create_lkas_hud_msg(
|
||||
self.packer, lkas_hud_msg, c.enabled, left_line, right_line, left_lane_depart, right_lane_depart))
|
||||
self.packer, lkas_hud_msg, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart))
|
||||
|
||||
if frame % 50 == 0:
|
||||
if self.frame % 50 == 0:
|
||||
can_sends.append(nissancan.create_lkas_hud_info_msg(
|
||||
self.packer, lkas_hud_info_msg, steer_hud_alert
|
||||
))
|
||||
@@ -83,4 +85,5 @@ class CarController():
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steeringAngleDeg = apply_angle
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
@@ -1,8 +1,9 @@
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.car.nissan.values import CAR
|
||||
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.nissan.values import CAR
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
|
||||
@@ -67,10 +68,4 @@ class CarInterface(CarInterfaceBase):
|
||||
return ret
|
||||
|
||||
def apply(self, c):
|
||||
hud_control = c.hudControl
|
||||
ret = self.CC.update(c, self.CS, self.frame, c.actuators,
|
||||
c.cruiseControl.cancel, hud_control.visualAlert,
|
||||
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
|
||||
hud_control.leftLaneDepart, hud_control.rightLaneDepart)
|
||||
self.frame += 1
|
||||
return ret
|
||||
return self.CC.update(c, self.CS)
|
||||
|
||||
@@ -1,10 +1,10 @@
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.subaru import subarucan
|
||||
from selfdrive.car.subaru.values import DBC, PREGLOBAL_CARS, CarControllerParams
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
|
||||
class CarController():
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.apply_steer_last = 0
|
||||
@@ -12,16 +12,20 @@ class CarController():
|
||||
self.es_lkas_cnt = -1
|
||||
self.cruise_button_prev = 0
|
||||
self.steer_rate_limited = False
|
||||
self.frame = 0
|
||||
|
||||
self.p = CarControllerParams(CP)
|
||||
self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
|
||||
|
||||
def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart):
|
||||
def update(self, CC, CS):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
|
||||
can_sends = []
|
||||
|
||||
# *** steering ***
|
||||
if (frame % self.p.STEER_STEP) == 0:
|
||||
if (self.frame % self.p.STEER_STEP) == 0:
|
||||
|
||||
apply_steer = int(round(actuators.steer * self.p.STEER_MAX))
|
||||
|
||||
@@ -31,13 +35,13 @@ class CarController():
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
if not c.latActive:
|
||||
if not CC.latActive:
|
||||
apply_steer = 0
|
||||
|
||||
if self.CP.carFingerprint in PREGLOBAL_CARS:
|
||||
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, frame, self.p.STEER_STEP))
|
||||
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, self.frame, self.p.STEER_STEP))
|
||||
else:
|
||||
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, self.p.STEER_STEP))
|
||||
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, self.frame, self.p.STEER_STEP))
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
|
||||
@@ -70,10 +74,11 @@ class CarController():
|
||||
self.es_distance_cnt = CS.es_distance_msg["Counter"]
|
||||
|
||||
if self.es_lkas_cnt != CS.es_lkas_msg["Counter"]:
|
||||
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, c.enabled, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart))
|
||||
can_sends.append(subarucan.create_es_lkas(self.packer, CS.es_lkas_msg, CC.enabled, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart))
|
||||
self.es_lkas_cnt = CS.es_lkas_msg["Counter"]
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steer = self.apply_steer_last / self.p.STEER_MAX
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
@@ -1,8 +1,9 @@
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.car.subaru.values import CAR, PREGLOBAL_CARS
|
||||
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.subaru.values import CAR, PREGLOBAL_CARS
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
|
||||
@@ -117,9 +118,4 @@ class CarInterface(CarInterfaceBase):
|
||||
return ret
|
||||
|
||||
def apply(self, c):
|
||||
hud_control = c.hudControl
|
||||
ret = self.CC.update(c, self.CS, self.frame, c.actuators,
|
||||
c.cruiseControl.cancel, hud_control.visualAlert,
|
||||
hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart)
|
||||
self.frame += 1
|
||||
return ret
|
||||
return self.CC.update(c, self.CS)
|
||||
|
||||
@@ -64,5 +64,4 @@ class CarInterface(CarInterfaceBase):
|
||||
return ret
|
||||
|
||||
def apply(self, c):
|
||||
ret = self.CC.update(c, self.CS)
|
||||
return ret
|
||||
return self.CC.update(c, self.CS)
|
||||
|
||||
@@ -274,5 +274,4 @@ class CarInterface(CarInterfaceBase):
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c):
|
||||
ret = self.CC.update(c, self.CS)
|
||||
return ret
|
||||
return self.CC.update(c, self.CS)
|
||||
|
||||
@@ -1,15 +1,17 @@
|
||||
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 opendbc.can.packer import CANPacker
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
class CarController():
|
||||
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.apply_steer_last = 0
|
||||
self.CP = CP
|
||||
self.apply_steer_last = 0
|
||||
self.frame = 0
|
||||
|
||||
self.packer_pt = CANPacker(DBC_FILES.mqb)
|
||||
|
||||
@@ -22,14 +24,15 @@ class CarController():
|
||||
|
||||
self.steer_rate_limited = False
|
||||
|
||||
def update(self, c, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart):
|
||||
""" Controls thread """
|
||||
def update(self, CC, CS, ext_bus):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
|
||||
can_sends = []
|
||||
|
||||
# **** Steering Controls ************************************************ #
|
||||
|
||||
if frame % P.HCA_STEP == 0:
|
||||
if self.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
|
||||
@@ -40,7 +43,7 @@ class CarController():
|
||||
# torque value. Do that anytime we happen to have 0 torque, or failing that,
|
||||
# when exceeding ~1/3 the 360 second timer.
|
||||
|
||||
if c.latActive:
|
||||
if CC.latActive:
|
||||
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
|
||||
@@ -66,34 +69,34 @@ class CarController():
|
||||
apply_steer = 0
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
idx = (frame / P.HCA_STEP) % 16
|
||||
idx = (self.frame / P.HCA_STEP) % 16
|
||||
can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_pt, CANBUS.pt, apply_steer,
|
||||
idx, hcaEnabled))
|
||||
|
||||
# **** HUD Controls ***************************************************** #
|
||||
|
||||
if frame % P.LDW_STEP == 0:
|
||||
if visual_alert in (VisualAlert.steerRequired, VisualAlert.ldw):
|
||||
if self.frame % P.LDW_STEP == 0:
|
||||
if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw):
|
||||
hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"]
|
||||
else:
|
||||
hud_alert = MQB_LDW_MESSAGES["none"]
|
||||
|
||||
can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, c.enabled,
|
||||
CS.out.steeringPressed, hud_alert, left_lane_visible,
|
||||
right_lane_visible, CS.ldw_stock_values,
|
||||
left_lane_depart, right_lane_depart))
|
||||
can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, CC.enabled,
|
||||
CS.out.steeringPressed, hud_alert, hud_control.leftLaneVisible,
|
||||
hud_control.rightLaneVisible, CS.ldw_stock_values,
|
||||
hud_control.leftLaneDepart, hud_control.rightLaneDepart))
|
||||
|
||||
# **** ACC Button Controls ********************************************** #
|
||||
|
||||
# FIXME: this entire section is in desperate need of refactoring
|
||||
|
||||
if self.CP.pcmCruise:
|
||||
if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
|
||||
if c.cruiseControl.cancel:
|
||||
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 c.enabled and CS.out.cruiseState.standstill:
|
||||
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()
|
||||
@@ -103,7 +106,7 @@ class CarController():
|
||||
self.graMsgBusCounterPrev = CS.graMsgBusCounter
|
||||
if self.graButtonStatesToSend is not None:
|
||||
if self.graMsgSentCount == 0:
|
||||
self.graMsgStartFramePrev = frame
|
||||
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
|
||||
@@ -114,4 +117,5 @@ class CarController():
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steer = self.apply_steer_last / P.STEER_MAX
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
@@ -193,12 +193,4 @@ class CarInterface(CarInterfaceBase):
|
||||
return ret
|
||||
|
||||
def apply(self, c):
|
||||
hud_control = c.hudControl
|
||||
ret = self.CC.update(c, self.CS, self.frame, self.ext_bus, c.actuators,
|
||||
hud_control.visualAlert,
|
||||
hud_control.leftLaneVisible,
|
||||
hud_control.rightLaneVisible,
|
||||
hud_control.leftLaneDepart,
|
||||
hud_control.rightLaneDepart)
|
||||
self.frame += 1
|
||||
return ret
|
||||
return self.CC.update(c, self.CS, self.ext_bus)
|
||||
|
||||
Reference in New Issue
Block a user