Toyota: keep Lane Sway Warning system (#24782)

* Toyota: pass through sway warning system signals

* signal doesn't seem to exist on the Prius V

bad log?

* remove duplicated checks

* syntax

* Revert "syntax"

This reverts commit 6c30c30d46e55a3595d7e2519b448d49c2f08a66.

* Revert "remove duplicated checks"

This reverts commit 6a364984eb8adfb0ee61b87452730da389dd5ff1.

* Revert "signal doesn't seem to exist on the Prius V"

This reverts commit 57fa571fde79336a48c50b617755fb2f68b2f124.

* Revert "Revert "signal doesn't seem to exist on the Prius V""

This reverts commit 9b1c3f6cacea74aa12a472bf02c469ffcd88eb24.

* Revert "Revert "remove duplicated checks""

This reverts commit 38bc7f3de6011e81c8e5f825a15dc9ec2fdcf5b0.

* Revert "Revert "syntax""

This reverts commit f81f93631f9a04406f818a2528680017635f229c.

* hard code values for Prius V

* send ui once every 25 frames

remove send ui logic

* send UI immediately on pcm_cancel_cmd

* draft

* clean that up

* same order as toyotacan

* update refs

Co-authored-by: Shane Smiskol <shane@smiskol.com>

selfdrive/car/toyota/carcontroller.py

@@ -146,7 +146,7 @@ class CarController:
       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))
+                                           hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud))
 
       if (self.frame % 100 == 0 or send_ui) and self.CP.enableDsu:
         can_sends.append(create_fcw_command(self.packer, fcw_alert))

selfdrive/car/toyota/carstate.py

@@ -1,3 +1,5 @@
+import copy
+
 from cereal import car
 from common.conversions import Conversions as CV
 from common.numpy_fast import mean
@@ -6,7 +8,7 @@ from common.realtime import DT_CTRL
 from opendbc.can.can_define import CANDefine
 from opendbc.can.parser import CANParser
 from selfdrive.car.interfaces import CarStateBase
-from selfdrive.car.toyota.values import ToyotaFlags, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
+from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
 
 
 class CarState(CarStateBase):
@@ -26,6 +28,7 @@ class CarState(CarStateBase):
 
     self.low_speed_lockout = False
     self.acc_type = 1
+    self.lkas_hud = {}
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
@@ -140,6 +143,9 @@ class CarState(CarStateBase):
       ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"] == 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1)
       ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"] == 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1)
 
+    if self.CP.carFingerprint != CAR.PRIUS_V:
+      self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
+
     return ret
 
   @staticmethod
@@ -254,6 +260,18 @@ class CarState(CarStateBase):
     signals = []
     checks = []
 
+    if CP.carFingerprint != CAR.PRIUS_V:
+      signals += [
+        ("LANE_SWAY_FLD", "LKAS_HUD"),
+        ("LANE_SWAY_BUZZER", "LKAS_HUD"),
+        ("LANE_SWAY_WARNING", "LKAS_HUD"),
+        ("LANE_SWAY_SENSITIVITY", "LKAS_HUD"),
+        ("LANE_SWAY_TOGGLE", "LKAS_HUD"),
+      ]
+      checks += [
+        ("LKAS_HUD", 1),
+      ]
+
     if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR):
       signals += [
         ("PRECOLLISION_ACTIVE", "PRE_COLLISION"),

selfdrive/car/toyota/toyotacan.py

@@ -66,13 +66,13 @@ def create_fcw_command(packer, fcw):
   return packer.make_can_msg("ACC_HUD", 0, values)
 
 
-def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled):
+def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled, stock_lkas_hud):
   values = {
     "TWO_BEEPS": chime,
     "LDA_ALERT": steer,
     "RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2,
     "LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
-    "BARRIERS" : 1 if enabled else 0,
+    "BARRIERS": 1 if enabled else 0,
 
     # static signals
     "SET_ME_X02": 2,
@@ -96,4 +96,9 @@ def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_dep
     "ADJUSTING_CAMERA": 0,
     "LDW_EXIST": 1,
   }
+
+  # lane sway functionality
+  # not all cars have LKAS_HUD — update with camera values if available
+  values.update(stock_lkas_hud)
+
   return packer.make_can_msg("LKAS_HUD", 0, values)

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-5f83a3ba20ec757ea7a5ef9b4159ac99cb77af0f
+67a4bd615017128ce04d0836608d2c7f32432e3e