sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Toyota: clean up CarInterface apply function (#23714)

Browse Source 
* clean up CarController with intermediate variables only accessed once or twice

* format

* fixes and move frame to CarController
pull/23982/head
Shane Smiskol 3 years ago committed by GitHub
parent 62bb43b7e2
commit facf6c109d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 32 additions and 33 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. 56
      selfdrive/car/toyota/carcontroller.py
  2. 9
      selfdrive/car/toyota/interface.py

56
selfdrive/car/toyota/carcontroller.py
Unescape View File

@ -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

9
selfdrive/car/toyota/interface.py
Unescape View File

@ -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

Write Preview
Loading…
Cancel
Save