diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 33e3fe118e..9b44c831f3 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -128,8 +128,8 @@ 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.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart, + CS.sws_toggle, CS.sws_sensitivity, CS.sws_buzzer, CS.sws_fld, CS.sws_warning, CC.enabled)) if self.frame % 100 == 0 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 281d01026f..fe5ae72a2c 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -128,6 +128,12 @@ 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) + self.sws_toggle = (cp_cam.vl["LKAS_HUD"]["LANE_SWAY_TOGGLE"]) + self.sws_sensitivity = (cp_cam.vl["LKAS_HUD"]["LANE_SWAY_SENSITIVITY"]) + self.sws_buzzer = (cp_cam.vl["LKAS_HUD"]["LANE_SWAY_BUZZER"]) + self.sws_fld = (cp_cam.vl["LKAS_HUD"]["LANE_SWAY_FLD"]) + self.sws_warning = (cp_cam.vl["LKAS_HUD"]["LANE_SWAY_WARNING"]) + return ret @staticmethod @@ -226,12 +232,18 @@ class CarState(CarStateBase): signals = [ ("FORCE", "PRE_COLLISION"), ("PRECOLLISION_ACTIVE", "PRE_COLLISION"), + ("LANE_SWAY_TOGGLE", "LKAS_HUD"), + ("LANE_SWAY_SENSITIVITY", "LKAS_HUD"), + ("LANE_SWAY_BUZZER", "LKAS_HUD"), + ("LANE_SWAY_FLD", "LKAS_HUD"), + ("LANE_SWAY_WARNING", "LKAS_HUD"), ] # use steering message to check if panda is connected to frc checks = [ ("STEERING_LKA", 42), ("PRE_COLLISION", 0), # TODO: figure out why freq is inconsistent + ("LKAS_HUD", 1), ] if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR): diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index d57131dcb9..835792545e 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -67,7 +67,7 @@ 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, sws_toggle, sws_sensitivity, sws_buzzer, sws_fld, sws_warning, enabled): values = { "TWO_BEEPS": chime, "LDA_ALERT": steer, @@ -75,18 +75,20 @@ def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_dep "LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2, "BARRIERS" : 1 if enabled else 0, + # signal pass through + "LANE_SWAY_FLD": sws_fld, + "LANE_SWAY_BUZZER": sws_buzzer, + "LANE_SWAY_WARNING": sws_warning, + "LANE_SWAY_SENSITIVITY": sws_sensitivity, + "LANE_SWAY_TOGGLE": sws_toggle, + # static signals "SET_ME_X02": 2, "SET_ME_X01": 1, "LKAS_STATUS": 1, "REPEATED_BEEPS": 0, - "LANE_SWAY_FLD": 7, - "LANE_SWAY_BUZZER": 0, - "LANE_SWAY_WARNING": 0, "LDA_FRONT_CAMERA_BLOCKED": 0, "TAKE_CONTROL": 0, - "LANE_SWAY_SENSITIVITY": 2, - "LANE_SWAY_TOGGLE": 1, "LDA_ON_MESSAGE": 0, "LDA_SPEED_TOO_LOW": 0, "LDA_SA_TOGGLE": 1,