mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-26 00:12:05 +08:00
Toyota: clean up CarInterface apply function (#23714)
* clean up CarController with intermediate variables only accessed once or twice * format * fixes and move frame to CarController
This commit is contained in:
Shane Smiskol
@@ -10,8 +10,9 @@ from opendbc.can.packer import CANPacker
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
class CarController():
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.frame = 0
|
||||
self.last_steer = 0
|
||||
self.alert_active = False
|
||||
self.last_standstill = False
|
||||
@@ -22,11 +23,13 @@ class CarController():
|
||||
self.gas = 0
|
||||
self.accel = 0
|
||||
|
||||
def update(self, c, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
|
||||
left_line, right_line, lead, left_lane_depart, right_lane_depart):
|
||||
def update(self, CC, CS):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
|
||||
# gas and brake
|
||||
if CS.CP.enableGasInterceptor and c.longActive:
|
||||
if CS.CP.enableGasInterceptor and CC.longActive:
|
||||
MAX_INTERCEPTOR_GAS = 0.5
|
||||
# RAV4 has very sensitive gas pedal
|
||||
if CS.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
|
||||
@@ -49,7 +52,7 @@ class CarController():
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
# Cut steering while we're in a known fault state (2s)
|
||||
if not c.latActive or CS.steer_state in (9, 25):
|
||||
if not CC.latActive or CS.steer_state in (9, 25):
|
||||
apply_steer = 0
|
||||
apply_steer_req = 0
|
||||
else:
|
||||
@@ -57,7 +60,7 @@ class CarController():
|
||||
|
||||
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
|
||||
# than CS.cruiseState.enabled. confirm they're not meaningfully different
|
||||
if not c.enabled and CS.pcm_acc_status:
|
||||
if not CC.enabled and CS.pcm_acc_status:
|
||||
pcm_cancel_cmd = 1
|
||||
|
||||
# on entering standstill, send standstill request
|
||||
@@ -72,24 +75,24 @@ class CarController():
|
||||
|
||||
can_sends = []
|
||||
|
||||
#*** control msgs ***
|
||||
#print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
|
||||
# *** control msgs ***
|
||||
# print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor)
|
||||
|
||||
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
|
||||
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
|
||||
# on consecutive messages
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
|
||||
if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
|
||||
can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2))
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, self.frame))
|
||||
if self.frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
|
||||
can_sends.append(create_lta_steer_command(self.packer, 0, 0, self.frame // 2))
|
||||
|
||||
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
|
||||
# if frame % 2 == 0:
|
||||
# can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
|
||||
# can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))
|
||||
# if self.frame % 2 == 0:
|
||||
# can_sends.append(create_steer_command(self.packer, 0, 0, self.frame // 2))
|
||||
# can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, self.frame // 2))
|
||||
|
||||
# we can spam can to cancel the system even if we are using lat only control
|
||||
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
|
||||
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged
|
||||
if (self.frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
|
||||
lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged
|
||||
|
||||
# Lexus IS uses a different cancellation message
|
||||
if pcm_cancel_cmd and CS.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
|
||||
@@ -100,17 +103,17 @@ class CarController():
|
||||
else:
|
||||
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type))
|
||||
|
||||
if frame % 2 == 0 and CS.CP.enableGasInterceptor and CS.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 2 == 0 and CS.CP.enableGasInterceptor and CS.CP.openpilotLongitudinalControl:
|
||||
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
|
||||
# This prevents unexpected pedal range rescaling
|
||||
can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2))
|
||||
can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2))
|
||||
self.gas = interceptor_gas_cmd
|
||||
|
||||
# ui mesg is at 1Hz but we send asap if:
|
||||
# - there is something to display
|
||||
# - there is something to stop displaying
|
||||
fcw_alert = hud_alert == VisualAlert.fcw
|
||||
steer_alert = hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw)
|
||||
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
|
||||
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
|
||||
|
||||
send_ui = False
|
||||
if ((fcw_alert or steer_alert) and not self.alert_active) or \
|
||||
@@ -121,15 +124,17 @@ class CarController():
|
||||
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
|
||||
send_ui = True
|
||||
|
||||
if (frame % 100 == 0 or send_ui):
|
||||
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart, c.enabled))
|
||||
if self.frame % 100 == 0 or send_ui:
|
||||
can_sends.append(create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible,
|
||||
hud_control.rightLaneVisible, hud_control.leftLaneDepart,
|
||||
hud_control.rightLaneDepart, CC.enabled))
|
||||
|
||||
if frame % 100 == 0 and CS.CP.enableDsu:
|
||||
if self.frame % 100 == 0 and CS.CP.enableDsu:
|
||||
can_sends.append(create_fcw_command(self.packer, fcw_alert))
|
||||
|
||||
# *** static msgs ***
|
||||
for (addr, cars, bus, fr_step, vl) in STATIC_DSU_MSGS:
|
||||
if frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
|
||||
for addr, cars, bus, fr_step, vl in STATIC_DSU_MSGS:
|
||||
if self.frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
|
||||
can_sends.append(make_can_msg(addr, vl, bus))
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
@@ -137,4 +142,5 @@ class CarController():
|
||||
new_actuators.accel = self.accel
|
||||
new_actuators.gas = self.gas
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
@@ -272,12 +272,5 @@ class CarInterface(CarInterfaceBase):
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
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.leadVisible,
|
||||
hud_control.leftLaneDepart, hud_control.rightLaneDepart)
|
||||
|
||||
self.frame += 1
|
||||
ret = self.CC.update(c, self.CS)
|
||||
return ret
|
||||
|
||||
Reference in New Issue
Block a user