diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 1f469c72be..6dbdd4b5d9 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/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)) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 3be748c4cf..050f8747a2 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/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"), diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 7ab3ab3e78..7e360cc4e1 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/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) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 342ab98b57..4f06673e97 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -5f83a3ba20ec757ea7a5ef9b4159ac99cb77af0f +67a4bd615017128ce04d0836608d2c7f32432e3e \ No newline at end of file