From 2f0f77a4f78516e4642b79245198dfaae39adbad Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 16 Mar 2022 20:06:17 -0700 Subject: [PATCH] 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 old-commit-hash: facf6c109d00be7fb15aa5ba35cc3df4b8046ce8 --- selfdrive/car/toyota/carcontroller.py | 56 +++++++++++++++------------ selfdrive/car/toyota/interface.py | 9 +---- 2 files changed, 32 insertions(+), 33 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 5ec69488d5..ad5edfb70d 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -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 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index aeac4ce2b2..e91ea4adc3 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -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