From 446b0c4b0d03b594bb5e37a26ee4e46458f5326d Mon Sep 17 00:00:00 2001 From: royjr Date: Tue, 10 Dec 2024 08:17:56 -0500 Subject: [PATCH] new --- opendbc/car/hyundai/carcontroller.py | 4 +- opendbc/car/hyundai/hyundaicanfd.py | 83 +++++++++++++++++++++++++--- 2 files changed, 78 insertions(+), 9 deletions(-) diff --git a/opendbc/car/hyundai/carcontroller.py b/opendbc/car/hyundai/carcontroller.py index aab3122be0..e25cd09f10 100644 --- a/opendbc/car/hyundai/carcontroller.py +++ b/opendbc/car/hyundai/carcontroller.py @@ -118,8 +118,8 @@ def update(self, CC, CS, now_nanos): if self.frame % 5 == 0 and (not hda2 or hda2_long) and self.car_fingerprint not in (CAR.HYUNDAI_SONATA_2024,): can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled)) if self.frame % 5 == 0 and (not hda2 or hda2_long) and self.car_fingerprint in (CAR.HYUNDAI_SONATA_2024,): - can_sends.append(hyundaicanfd.create_msg_161(self.packer, self.CAN, CC.enabled, CS.msg_161)) - can_sends.append(hyundaicanfd.create_msg_162(self.packer, self.CAN, CC.enabled, CS.msg_162)) + can_sends.append(hyundaicanfd.create_msg_161(self.packer, self.CAN, CC.enabled, CS.msg_161, self.CP, hud_control, CS, CC, self.frame)) + can_sends.append(hyundaicanfd.create_msg_162(self.packer, self.CAN, CC.enabled, CS.msg_162, self.CP, hud_control)) # blinkers if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: diff --git a/opendbc/car/hyundai/hyundaicanfd.py b/opendbc/car/hyundai/hyundaicanfd.py index 62d8f1694b..bdc1c626fb 100644 --- a/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc/car/hyundai/hyundaicanfd.py @@ -1,4 +1,5 @@ from opendbc.car import CanBusBase +from opendbc.car.common.conversions import Conversions as CV from opendbc.car.common.numpy_fast import clip from opendbc.car.hyundai.values import HyundaiFlags @@ -119,29 +120,97 @@ def create_lfahda_cluster(packer, CAN, enabled): } return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) -def create_msg_161(packer, CAN, enabled, msg_161): +def create_msg_161(packer, CAN, enabled, msg_161, car_params, hud_control, car_state, car_control, frame): values = msg_161.copy() + + # HIDE ALERTS + if values.get("ALERTS_5") == 5: # USE SWITCH OR PEDAL TO ACCELERATE + values["ALERTS_5"] = 0 + if values.get("ALERTS_2") == 5: # CONSIDER TAKING A BREAK + values.update({"ALERTS_2": 0, "SOUNDS_2": 0, "DAW_ICON": 0}) + + # LANELINES + curvature = { + i: (31 if i == -1 else 13 - abs(i + 15)) if i < 0 else 15 + i + for i in range(-15, 16) + } values.update({ + "LANELINE_CURVATURE": curvature.get(max(-15, min(int(car_state.out.steeringAngleDeg / 3), 15)), 14) if enabled else 15, "LFA_ICON": 2 if enabled else 0, "LKA_ICON": 4 if enabled else 0, "LANELINE_LEFT": 2 if enabled else 0, "LANELINE_RIGHT": 2 if enabled else 0, "CENTERLINE": 1 if enabled else 0, - "DAW_ICON": 0, }) - if values.get("ALERTS_5") == 5: # use_switch_or_pedal_to_accelerate - values["ALERTS_5"] = 0 - if values.get("ALERTS_2") == 5: # coffee - values.update({"ALERTS_2": 0, "SOUNDS_2": 0}) + + # LCA + if enabled: + speed_below_threshold = car_state.out.vEgo < 8.94 + values.update({ + "LCA_LEFT_ICON": 0 if car_state.out.leftBlindspot or speed_below_threshold else 2 if car_control.leftBlinker else 1, + "LCA_RIGHT_ICON": 0 if car_state.out.rightBlindspot or speed_below_threshold else 2 if car_control.rightBlinker else 1, + "LCA_LEFT_ARROW": 2 if car_control.leftBlinker else 0, + "LCA_RIGHT_ARROW": 2 if car_control.rightBlinker else 0, + }) + + # LANE DEPARTURE + if hud_control.leftLaneDepart: + values["LANELINE_LEFT"] = 4 if (frame // 50) % 2 == 0 else 1 + if hud_control.rightLaneDepart: + values["LANELINE_RIGHT"] = 4 if (frame // 50) % 2 == 0 else 1 + + if car_params.openpilotLongitudinalControl: + # HIDE ALERTS + if values.get("ALERTS_5") == 4: # SMART CRUISE CONTROL CONDITIONS NOT MET + values["ALERTS_5"] = 0 + + # SETSPEED + values["SETSPEED"] = 3 if enabled else 1 + values["SETSPEED_HUD"] = 2 if enabled else 1 + values["SETSPEED_SPEED"] = 25 if (s := round(car_state.out.vCruiseCluster * CV.KPH_TO_MPH)) > 100 else s + + # DISTANCE + if 1 <= hud_control.leadDistanceBars <= 3: + values["DISTANCE"] = hud_control.leadDistanceBars + values["DISTANCE_SPACING"] = 1 if enabled else 0 + values["DISTANCE_LEAD"] = 2 if enabled and hud_control.leadVisible else 1 if enabled else 0 + values["DISTANCE_CAR"] = 2 if enabled else 1 + values["ALERTS_3"] = hud_control.leadDistanceBars + 6 + else: + values["DISTANCE"] = 0 + values["DISTANCE_SPACING"] = 0 + values["DISTANCE_LEAD"] = 0 + values["DISTANCE_CAR"] = 0 + + # BACKGROUND + values["BACKGROUND"] = 1 if enabled else 7 + return packer.make_can_msg("MSG_161", CAN.ECAN, values) -def create_msg_162(packer, CAN, enabled, msg_162): +def create_msg_162(packer, CAN, enabled, msg_162, car_params, hud_control): values = msg_162.copy() + + # HIDE FAULTS values.update({ "FAULT_LSS": 0, "FAULT_HDA": 0, "FAULT_DAS": 0, }) + + # LANE DEPARTURE + if hud_control.leftLaneDepart or hud_control.rightLaneDepart: + values["VIBRATE"] = 1 + + if car_params.openpilotLongitudinalControl: + # *** TODO *** LEAD_DISTANCE/LEAD_LATERAL + # LEAD + if hud_control.leadVisible: + values["LEAD"] = 2 if enabled else 1 + values["LEAD_DISTANCE"] = 100 + else: + values["LEAD"] = 0 + values["LEAD_DISTANCE"] = 0 + return packer.make_can_msg("MSG_162", CAN.ECAN, values) def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control):