From f6e31e8de541efb670eb9b14fff99c11aabd9d27 Mon Sep 17 00:00:00 2001 From: Irene <12470297+cydia2020@users.noreply.github.com> Date: Sat, 26 Aug 2023 13:17:05 +1000 Subject: [PATCH] Toyota: forward openpilot FCW to cluster when engaged (#25677) * move ui message handling out of condition * relay openpilot fcw to car * only send normal fcw if DSU is unplugged * add comment --------- Co-authored-by: Shane Smiskol old-commit-hash: b3e4eb15dc7a41385a7bbd26a3cbba10a8139f2d --- selfdrive/car/toyota/carcontroller.py | 11 ++++++----- selfdrive/car/toyota/toyotacan.py | 3 ++- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index b765d70b9a..01896f1421 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -125,6 +125,10 @@ class CarController: self.last_standstill = CS.out.standstill + # handle UI messages + fcw_alert = hud_control.visualAlert == VisualAlert.fcw + steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) + # we can spam can to cancel the system even if we are using lat only control if (self.frame % 3 == 0 and self.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 @@ -133,10 +137,10 @@ class CarController: if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR: can_sends.append(create_acc_cancel_command(self.packer)) elif self.CP.openpilotLongitudinalControl: - can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type)) + can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert)) self.accel = pcm_accel_cmd else: - can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type)) + can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False)) if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl: # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. @@ -149,9 +153,6 @@ class CarController: # ui mesg is at 1Hz but we send asap if: # - there is something to display # - there is something to stop displaying - 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 \ (not (fcw_alert or steer_alert) and self.alert_active): diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 0792ac869f..ed0237c1be 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -27,7 +27,7 @@ def create_lta_steer_command(packer, steer_angle, steer_req, frame, setme_x64): return packer.make_can_msg("STEERING_LTA", 0, values) -def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type): +def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert): # TODO: find the exact canceling bit that does not create a chime values = { "ACCEL_CMD": accel, @@ -38,6 +38,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty "RELEASE_STANDSTILL": not standstill_req, "CANCEL_REQ": pcm_cancel, "ALLOW_LONG_PRESS": 1, + "ACC_CUT_IN": fcw_alert, # only shown when ACC enabled } return packer.make_can_msg("ACC_CONTROL", 0, values)