From 5d399457c8542b3a4cf04caeb2f1012007e2ff9f Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Mon, 16 Dec 2024 09:33:18 +0100 Subject: [PATCH] Modular Assistive Driving System (MADS) (#29) * fix * clearer * cleanup * more fix * hkg dbc * typo! * Add Custom MIT License (#26) * missed * better * more fixes * try this out * inherit in carcontroller properly * hyundai: main button handling * always disable * add main check for pcm cruise * revert * sunnyParams * Move car-specific changes to opendbc * no need * more fixes * more! * final? * static analysis * use new cereal * rename to lkas_button * rename * mads base for cars * add lkas for ford * enabled <-> active * MUST REMOVE test process replay * Revert "MUST REMOVE test process replay" This reverts commit 6dde2c8435b0e09158ab455aa215a573f5212c11. * subaru * ruff * more subaru * toyota * add them * mypy * fix * update name * FCA * assign directly * init directly * missing * not yet * missed * no longer needed * missed hd * fix * move to generator * more nissan * Apply suggestions from code review * no need * Revert "no need" This reverts commit 6156c62113d9abb626014947a9066b5580f6460a. * hyundai: move main logic out of main carstate * move around * move lkas and lfa icon logic to mads base * Parse more flags from alt exp, more tests, hyundai main cruise allowed * license * add code spell ignore * fix icon * remove toyota lta status for lkas, causes weird behaviors * parse signals inside mads methods * more codes in mads childs * Update opendbc/sunnypilot/car/hyundai/escc.py * revert * type hint * test type hint * more type hint * no * needs to be in carstate * in another PR --------- Co-authored-by: Jason Wen --- .codespellignore | 1 + .pre-commit-config.yaml | 2 + opendbc/car/car.capnp | 2 +- opendbc/car/chrysler/carcontroller.py | 13 +- opendbc/car/chrysler/carstate.py | 16 ++- opendbc/car/chrysler/chryslercan.py | 7 +- opendbc/car/ford/carstate.py | 13 +- opendbc/car/hyundai/carcontroller.py | 19 ++- opendbc/car/hyundai/carstate.py | 21 ++- opendbc/car/hyundai/hyundaican.py | 15 ++- opendbc/car/hyundai/hyundaicanfd.py | 12 +- opendbc/car/hyundai/interface.py | 3 + opendbc/car/nissan/interface.py | 4 + opendbc/car/subaru/carstate.py | 9 +- opendbc/car/toyota/carstate.py | 16 ++- opendbc/sunnypilot/__init__.py | 5 + opendbc/sunnypilot/car/chrysler/__init__.py | 0 opendbc/sunnypilot/car/chrysler/mads.py | 114 ++++++++++++++++ opendbc/sunnypilot/car/ford/__init__.py | 0 opendbc/sunnypilot/car/ford/mads.py | 43 ++++++ opendbc/sunnypilot/car/hyundai/mads.py | 137 ++++++++++++++++++++ opendbc/sunnypilot/car/hyundai/values.py | 28 ++++ opendbc/sunnypilot/car/subaru/__init__.py | 0 opendbc/sunnypilot/car/subaru/mads.py | 67 ++++++++++ opendbc/sunnypilot/car/toyota/__init__.py | 0 opendbc/sunnypilot/car/toyota/mads.py | 80 ++++++++++++ opendbc/sunnypilot/mads_base.py | 43 ++++++ 27 files changed, 630 insertions(+), 40 deletions(-) create mode 100644 .codespellignore create mode 100644 opendbc/sunnypilot/car/chrysler/__init__.py create mode 100644 opendbc/sunnypilot/car/chrysler/mads.py create mode 100644 opendbc/sunnypilot/car/ford/__init__.py create mode 100644 opendbc/sunnypilot/car/ford/mads.py create mode 100644 opendbc/sunnypilot/car/hyundai/mads.py create mode 100644 opendbc/sunnypilot/car/subaru/__init__.py create mode 100644 opendbc/sunnypilot/car/subaru/mads.py create mode 100644 opendbc/sunnypilot/car/toyota/__init__.py create mode 100644 opendbc/sunnypilot/car/toyota/mads.py create mode 100644 opendbc/sunnypilot/mads_base.py diff --git a/.codespellignore b/.codespellignore new file mode 100644 index 00000000..91616934 --- /dev/null +++ b/.codespellignore @@ -0,0 +1 @@ +Wen diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 03afd741..89fb6db9 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -13,6 +13,8 @@ repos: hooks: - id: codespell exclude: '(\.dbc|LICENSE\.md)$' + args: + - --ignore-words=.codespellignore - repo: https://github.com/pre-commit/mirrors-mypy rev: v1.11.1 hooks: diff --git a/opendbc/car/car.capnp b/opendbc/car/car.capnp index e10ad8ed..5a899425 100644 --- a/opendbc/car/car.capnp +++ b/opendbc/car/car.capnp @@ -351,6 +351,7 @@ struct CarControl { cruiseControl @4 :CruiseControl; hudControl @5 :HUDControl; + madsEnabled @7 :Bool; sunnypilotParams @8 :UInt32; struct Actuators { @@ -430,7 +431,6 @@ struct CarControl { gasDEPRECATED @1 :Float32; brakeDEPRECATED @2 :Float32; steeringTorqueDEPRECATED @3 :Float32; - activeDEPRECATED @7 :Bool; pitchDEPRECATED @9 :Float32; actuatorsOutputDEPRECATED @10 :Actuators; } diff --git a/opendbc/car/chrysler/carcontroller.py b/opendbc/car/chrysler/carcontroller.py index 62ceb154..b86210d3 100644 --- a/opendbc/car/chrysler/carcontroller.py +++ b/opendbc/car/chrysler/carcontroller.py @@ -4,10 +4,13 @@ from opendbc.car.chrysler import chryslercan from opendbc.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags from opendbc.car.interfaces import CarControllerBase +from opendbc.sunnypilot.car.chrysler.mads import MadsCarController -class CarController(CarControllerBase): + +class CarController(CarControllerBase, MadsCarController): def __init__(self, dbc_names, CP): - super().__init__(dbc_names, CP) + CarControllerBase.__init__(self, dbc_names, CP) + MadsCarController.__init__(self) self.apply_steer_last = 0 self.hud_count = 0 @@ -19,6 +22,7 @@ class CarController(CarControllerBase): self.params = CarControllerParams(CP) def update(self, CC, CS, now_nanos): + MadsCarController.update(self, CC, CS) can_sends = [] lkas_active = CC.latActive and self.lkas_control_bit_prev @@ -41,7 +45,7 @@ class CarController(CarControllerBase): if self.frame % 25 == 0: if CS.lkas_car_model != -1: can_sends.append(chryslercan.create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert, - self.hud_count, CS.lkas_car_model, CS.auto_high_beam)) + self.hud_count, CS.lkas_car_model, CS.auto_high_beam, self.mads)) self.hud_count += 1 # steering @@ -74,6 +78,9 @@ class CarController(CarControllerBase): can_sends.append(chryslercan.create_lkas_command(self.packer, self.CP, int(apply_steer), lkas_control_bit)) + if self.frame % 10 == 0 and self.CP.carFingerprint not in RAM_CARS: + can_sends.append(MadsCarController.create_lkas_heartbit(self.packer, CS.lkas_heartbit, self.mads)) + self.frame += 1 new_actuators = CC.actuators.as_builder() diff --git a/opendbc/car/chrysler/carstate.py b/opendbc/car/chrysler/carstate.py index 65d7f7ae..dd2bb36d 100644 --- a/opendbc/car/chrysler/carstate.py +++ b/opendbc/car/chrysler/carstate.py @@ -5,12 +5,15 @@ from opendbc.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS from opendbc.car.common.conversions import Conversions as CV from opendbc.car.interfaces import CarStateBase +from opendbc.sunnypilot.car.chrysler.mads import MadsCarState + ButtonType = structs.CarState.ButtonEvent.Type -class CarState(CarStateBase): +class CarState(CarStateBase, MadsCarState): def __init__(self, CP): - super().__init__(CP) + CarStateBase.__init__(self, CP) + MadsCarState.__init__(self, CP) self.CP = CP can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) @@ -104,7 +107,12 @@ class CarState(CarStateBase): self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"] self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"] - ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + MadsCarState.update_mads(self, ret, can_parsers) + + ret.buttonEvents = [ + *create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas}), + ] return ret @@ -154,6 +162,8 @@ class CarState(CarStateBase): if CP.carFingerprint in RAM_CARS: cam_messages += CarState.get_cruise_messages() + MadsCarState.get_parser(CP, pt_messages, cam_messages) + return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0), Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, 2), diff --git a/opendbc/car/chrysler/chryslercan.py b/opendbc/car/chrysler/chryslercan.py index 8a0755ac..ad9e1b27 100644 --- a/opendbc/car/chrysler/chryslercan.py +++ b/opendbc/car/chrysler/chryslercan.py @@ -4,7 +4,7 @@ from opendbc.car.chrysler.values import RAM_CARS GearShifter = structs.CarState.GearShifter VisualAlert = structs.CarControl.HUDControl.VisualAlert -def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam): +def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam, mads): # LKAS_HUD - Controls what lane-keeping icon is displayed # == Color == @@ -27,7 +27,10 @@ def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, au # 7 Normal # 6 lane departure place hands on wheel - color = 2 if lkas_active else 1 + if mads.enable_mads: + color = 2 if lkas_active else 1 if mads.paused else 0 + else: + color = 2 if lkas_active else 1 lines = 3 if lkas_active else 0 alerts = 7 if lkas_active else 0 diff --git a/opendbc/car/ford/carstate.py b/opendbc/car/ford/carstate.py index 620f0e6a..5e921a4c 100644 --- a/opendbc/car/ford/carstate.py +++ b/opendbc/car/ford/carstate.py @@ -6,14 +6,17 @@ from opendbc.car.ford.fordcan import CanBus from opendbc.car.ford.values import DBC, CarControllerParams, FordFlags from opendbc.car.interfaces import CarStateBase +from opendbc.sunnypilot.car.ford.mads import MadsCarState + ButtonType = structs.CarState.ButtonEvent.Type GearShifter = structs.CarState.GearShifter TransmissionType = structs.CarParams.TransmissionType -class CarState(CarStateBase): +class CarState(CarStateBase, MadsCarState): def __init__(self, CP): - super().__init__(CP) + CarStateBase.__init__(self, CP) + MadsCarState.__init__(self, CP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) if CP.transmissionType == TransmissionType.automatic: self.shifter_values = can_define.dv["PowertrainData_10"]["TrnRng_D_Rq"] @@ -110,7 +113,11 @@ class CarState(CarStateBase): self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"] self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"] - ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + MadsCarState.update_mads(self, ret, can_parsers) + + ret.buttonEvents = [ + *create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}), + *create_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas})] return ret diff --git a/opendbc/car/hyundai/carcontroller.py b/opendbc/car/hyundai/carcontroller.py index 4d6eef8f..78e71029 100644 --- a/opendbc/car/hyundai/carcontroller.py +++ b/opendbc/car/hyundai/carcontroller.py @@ -9,6 +9,7 @@ from opendbc.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParam from opendbc.car.interfaces import CarControllerBase from opendbc.sunnypilot.car.hyundai.escc import EsccCarController +from opendbc.sunnypilot.car.hyundai.mads import MadsCarController VisualAlert = structs.CarControl.HUDControl.VisualAlert LongCtrlState = structs.CarControl.Actuators.LongControlState @@ -44,10 +45,11 @@ def process_hud_alert(enabled, fingerprint, hud_control): return sys_warning, sys_state, left_lane_warning, right_lane_warning -class CarController(CarControllerBase, EsccCarController): +class CarController(CarControllerBase, EsccCarController, MadsCarController): def __init__(self, dbc_names, CP): CarControllerBase.__init__(self, dbc_names, CP) EsccCarController.__init__(self, CP) + MadsCarController.__init__(self) self.CAN = CanBus(CP) self.params = CarControllerParams(CP) self.packer = CANPacker(dbc_names[Bus.pt]) @@ -60,6 +62,7 @@ class CarController(CarControllerBase, EsccCarController): def update(self, CC, CS, now_nanos): EsccCarController.update(self, CS) + MadsCarController.update(self, self.CP, CC, self.frame) actuators = CC.actuators hud_control = CC.hudControl @@ -112,7 +115,7 @@ class CarController(CarControllerBase, EsccCarController): hda2_long = hda2 and self.CP.openpilotLongitudinalControl # steering control - can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer)) + can_sends.extend(hyundaicanfd.create_steering_messages(self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_steer, self.lkas_icon)) # prevent LFA from activating on HDA2 by sending "no lane lines detected" to ADAS ECU if self.frame % 5 == 0 and hda2: @@ -121,7 +124,7 @@ class CarController(CarControllerBase, EsccCarController): # LFA and HDA icons if self.frame % 5 == 0 and (not hda2 or hda2_long): - can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled)) + can_sends.append(hyundaicanfd.create_lfahda_cluster(self.packer, self.CAN, CC.enabled, self.lfa_icon)) # blinkers if hda2 and self.CP.flags & HyundaiFlags.ENABLE_BLINKERS: @@ -132,7 +135,8 @@ class CarController(CarControllerBase, EsccCarController): can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame)) if self.frame % 2 == 0: can_sends.append(hyundaicanfd.create_acc_control(self.packer, self.CAN, CC.enabled, self.accel_last, accel, stopping, CC.cruiseControl.override, - set_speed_in_units, hud_control)) + set_speed_in_units, hud_control, + CS.main_cruise_enabled)) self.accel_last = accel else: # button presses @@ -141,7 +145,8 @@ class CarController(CarControllerBase, EsccCarController): can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.CP, apply_steer, apply_steer_req, torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled, hud_control.leftLaneVisible, hud_control.rightLaneVisible, - left_lane_warning, right_lane_warning)) + left_lane_warning, right_lane_warning, + self.lkas_icon)) if not self.CP.openpilotLongitudinalControl: can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True)) @@ -153,11 +158,11 @@ class CarController(CarControllerBase, EsccCarController): can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), hud_control, set_speed_in_units, stopping, CC.cruiseControl.override, use_fca, - self.ESCC)) + CS.main_cruise_enabled, self.ESCC)) # 20 Hz LFA MFA message if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value: - can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled)) + can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled, self.lfa_icon)) # 5 Hz ACC options if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl: diff --git a/opendbc/car/hyundai/carstate.py b/opendbc/car/hyundai/carstate.py index a1a419a6..ef1247fd 100644 --- a/opendbc/car/hyundai/carstate.py +++ b/opendbc/car/hyundai/carstate.py @@ -11,6 +11,7 @@ from opendbc.car.hyundai.values import HyundaiFlags, CAR, DBC, Buttons, CarContr from opendbc.car.interfaces import CarStateBase from opendbc.sunnypilot.car.hyundai.escc import EsccCarStateBase +from opendbc.sunnypilot.car.hyundai.mads import MadsCarState ButtonType = structs.CarState.ButtonEvent.Type @@ -22,10 +23,11 @@ BUTTONS_DICT = {Buttons.RES_ACCEL: ButtonType.accelCruise, Buttons.SET_DECEL: Bu Buttons.GAP_DIST: ButtonType.gapAdjustCruise, Buttons.CANCEL: ButtonType.cancel} -class CarState(CarStateBase, EsccCarStateBase): +class CarState(CarStateBase, EsccCarStateBase, MadsCarState): def __init__(self, CP): CarStateBase.__init__(self, CP) EsccCarStateBase.__init__(self) + MadsCarState.__init__(self, CP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) self.cruise_buttons: deque = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) @@ -178,8 +180,14 @@ class CarState(CarStateBase, EsccCarStateBase): self.cruise_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwState"]) self.main_buttons.extend(cp.vl_all["CLU11"]["CF_Clu_CruiseSwMain"]) + MadsCarState.update_mads(self, ret, can_parsers) + ret.buttonEvents = [*create_button_events(self.cruise_buttons[-1], prev_cruise_buttons, BUTTONS_DICT), - *create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise})] + *create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise}), + *create_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas})] + + if self.CP.openpilotLongitudinalControl: + ret.cruiseState.available = self.get_main_cruise(ret) return ret @@ -267,8 +275,14 @@ class CarState(CarStateBase, EsccCarStateBase): self.hda2_lfa_block_msg = copy.copy(cp_cam.vl["CAM_0x362"] if self.CP.flags & HyundaiFlags.CANFD_HDA2_ALT_STEERING else cp_cam.vl["CAM_0x2a4"]) + MadsCarState.update_mads_canfd(self, ret, can_parsers) + ret.buttonEvents = [*create_button_events(self.cruise_buttons[-1], prev_cruise_buttons, BUTTONS_DICT), - *create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise})] + *create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise}), + *create_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas})] + + if self.CP.openpilotLongitudinalControl: + ret.cruiseState.available = self.get_main_cruise(ret) return ret @@ -384,6 +398,7 @@ class CarState(CarStateBase, EsccCarStateBase): if CP.flags & HyundaiFlags.USE_FCA.value: cam_messages.append(("FCA11", 50)) + MadsCarState.get_parser(CP, pt_messages) return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0), diff --git a/opendbc/car/hyundai/hyundaican.py b/opendbc/car/hyundai/hyundaican.py index b68e3392..befa4755 100644 --- a/opendbc/car/hyundai/hyundaican.py +++ b/opendbc/car/hyundai/hyundaican.py @@ -8,7 +8,8 @@ hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf) def create_lkas11(packer, frame, CP, apply_steer, steer_req, torque_fault, lkas11, sys_warning, sys_state, enabled, left_lane, right_lane, - left_lane_depart, right_lane_depart): + left_lane_depart, right_lane_depart, + lkas_icon): values = {s: lkas11[s] for s in [ "CF_Lkas_LdwsActivemode", "CF_Lkas_LdwsSysState", @@ -50,7 +51,7 @@ def create_lkas11(packer, frame, CP, apply_steer, steer_req, # FcwOpt_USM 2 = Green car + lanes # FcwOpt_USM 1 = White car + lanes # FcwOpt_USM 0 = No car + lanes - values["CF_Lkas_FcwOpt_USM"] = 2 if enabled else 1 + values["CF_Lkas_FcwOpt_USM"] = lkas_icon # SysWarning 4 = keep hands on wheel # SysWarning 5 = keep hands on wheel (red) @@ -67,7 +68,7 @@ def create_lkas11(packer, frame, CP, apply_steer, steer_req, # SysState 1-2 = white car + lanes # SysState 3 = green car + lanes, green steering wheel # SysState 4 = green car + lanes - values["CF_Lkas_LdwsSysState"] = 3 if enabled else 1 + values["CF_Lkas_LdwsSysState"] = lkas_icon values["CF_Lkas_LdwsOpt_USM"] = 2 # non-2 changes above SysState definition # these have no effect @@ -119,18 +120,18 @@ def create_clu11(packer, frame, clu11, button, CP): return packer.make_can_msg("CLU11", bus, values) -def create_lfahda_mfc(packer, enabled): +def create_lfahda_mfc(packer, enabled, lfa_icon): values = { - "LFA_Icon_State": 2 if enabled else 0, + "LFA_Icon_State": lfa_icon, } return packer.make_can_msg("LFAHDA_MFC", 0, values) def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, - ESCC: EnhancedSmartCruiseControl = None): + main_cruise_enabled, ESCC: EnhancedSmartCruiseControl = None): def get_scc11_values(): return { - "MainMode_ACC": 1, + "MainMode_ACC": 1 if main_cruise_enabled else 0, "TauGapSet": hud_control.leadDistanceBars, "VSetDis": set_speed if enabled else 0, "AliveCounterACC": idx % 0x10, diff --git a/opendbc/car/hyundai/hyundaicanfd.py b/opendbc/car/hyundai/hyundaicanfd.py index d3488ff5..35dbe96c 100644 --- a/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc/car/hyundai/hyundaicanfd.py @@ -34,13 +34,13 @@ class CanBus(CanBusBase): return self._cam -def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer): +def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_steer, lkas_icon): ret = [] values = { "LKA_MODE": 2, - "LKA_ICON": 2 if enabled else 1, + "LKA_ICON": lkas_icon, "TORQUE_REQUEST": apply_steer, "LKA_ASSIST": 0, "STEER_REQ": 1 if lat_active else 0, @@ -112,15 +112,15 @@ def create_acc_cancel(packer, CP, CAN, cruise_info_copy): }) return packer.make_can_msg("SCC_CONTROL", CAN.ECAN, values) -def create_lfahda_cluster(packer, CAN, enabled): +def create_lfahda_cluster(packer, CAN, enabled, lfa_icon): values = { "HDA_ICON": 1 if enabled else 0, - "LFA_ICON": 2 if enabled else 0, + "LFA_ICON": lfa_icon, } return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values) -def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control): +def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, main_cruise_enabled): jerk = 5 jn = jerk / 50 if not enabled or gas_override: @@ -131,7 +131,7 @@ def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_ov values = { "ACCMode": 0 if not enabled else (2 if gas_override else 1), - "MainMode_ACC": 1, + "MainMode_ACC": 1 if main_cruise_enabled else 0, "StopReq": 1 if stopping else 0, "aReqValue": a_val, "aReqRaw": a_raw, diff --git a/opendbc/car/hyundai/interface.py b/opendbc/car/hyundai/interface.py index b06b5f09..9419990e 100644 --- a/opendbc/car/hyundai/interface.py +++ b/opendbc/car/hyundai/interface.py @@ -95,6 +95,9 @@ class CarInterface(CarInterfaceBase): if ret.flags & HyundaiFlags.CAMERA_SCC: ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_CAMERA_SCC + if 0x391 in fingerprint[0]: + ret.sunnypilotFlags |= HyundaiFlagsSP.HAS_LFA_BUTTON.value + # Common lateral control setup ret.centerToFront = ret.wheelbase * 0.4 diff --git a/opendbc/car/nissan/interface.py b/opendbc/car/nissan/interface.py index 14255338..e6ba0fd1 100644 --- a/opendbc/car/nissan/interface.py +++ b/opendbc/car/nissan/interface.py @@ -23,4 +23,8 @@ class CarInterface(CarInterfaceBase): # Altima has EPS on C-CAN unlike the others that have it on V-CAN ret.safetyConfigs[0].safetyParam |= Panda.FLAG_NISSAN_ALT_EPS_BUS + # Used for panda safety and tests + if candidate in (CAR.NISSAN_LEAF, CAR.NISSAN_LEAF_IC): + ret.safetyConfigs[-1].safetyParam |= Panda.FLAG_NISSAN_LEAF + return ret diff --git a/opendbc/car/subaru/carstate.py b/opendbc/car/subaru/carstate.py index 5e4343c2..29e0aff5 100644 --- a/opendbc/car/subaru/carstate.py +++ b/opendbc/car/subaru/carstate.py @@ -7,10 +7,13 @@ from opendbc.car.interfaces import CarStateBase from opendbc.car.subaru.values import DBC, CanBus, SubaruFlags from opendbc.car import CanSignalRateCalculator +from opendbc.sunnypilot.car.subaru.mads import MadsCarState -class CarState(CarStateBase): + +class CarState(CarStateBase, MadsCarState): def __init__(self, CP): - super().__init__(CP) + CarStateBase.__init__(self, CP) + MadsCarState.__init__(self, CP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) self.shifter_values = can_define.dv["Transmission"]["Gear"] @@ -128,6 +131,8 @@ class CarState(CarStateBase): if self.CP.flags & SubaruFlags.SEND_INFOTAINMENT: self.es_infotainment_msg = copy.copy(cp_cam.vl["ES_Infotainment"]) + MadsCarState.update_mads(self, ret, can_parsers) + return ret @staticmethod diff --git a/opendbc/car/toyota/carstate.py b/opendbc/car/toyota/carstate.py index adf32045..ded98460 100644 --- a/opendbc/car/toyota/carstate.py +++ b/opendbc/car/toyota/carstate.py @@ -10,6 +10,8 @@ from opendbc.car.interfaces import CarStateBase from opendbc.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \ TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR +from opendbc.sunnypilot.car.toyota.mads import MadsCarState + ButtonType = structs.CarState.ButtonEvent.Type SteerControlType = structs.CarParams.SteerControlType @@ -25,9 +27,10 @@ TEMP_STEER_FAULTS = (0, 9, 11, 21, 25) PERM_STEER_FAULTS = (3, 17) -class CarState(CarStateBase): +class CarState(CarStateBase, MadsCarState): def __init__(self, CP): - super().__init__(CP) + CarStateBase.__init__(self, CP) + MadsCarState.__init__(self, CP) can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100. self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2. @@ -188,7 +191,14 @@ class CarState(CarStateBase): prev_distance_button = self.distance_button self.distance_button = cp_acc.vl["ACC_CONTROL"]["DISTANCE"] - ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + self.distance_button_events = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}) + + MadsCarState.update_mads(self, ret, can_parsers) + + ret.buttonEvents = [ + *self.distance_button_events, + *self.lkas_button_events, + ] return ret diff --git a/opendbc/sunnypilot/__init__.py b/opendbc/sunnypilot/__init__.py index e69de29b..208ddb7c 100644 --- a/opendbc/sunnypilot/__init__.py +++ b/opendbc/sunnypilot/__init__.py @@ -0,0 +1,5 @@ +from enum import IntFlag + + +class SunnypilotParamFlags(IntFlag): + ENABLE_MADS = 1 diff --git a/opendbc/sunnypilot/car/chrysler/__init__.py b/opendbc/sunnypilot/car/chrysler/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/opendbc/sunnypilot/car/chrysler/mads.py b/opendbc/sunnypilot/car/chrysler/mads.py new file mode 100644 index 00000000..8bd4f59b --- /dev/null +++ b/opendbc/sunnypilot/car/chrysler/mads.py @@ -0,0 +1,114 @@ +""" +The MIT License + +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + +Last updated: July 29, 2024 +""" + +from enum import StrEnum +from collections import namedtuple + +from opendbc.car import Bus, structs +from opendbc.car.chrysler.values import RAM_CARS + +from opendbc.sunnypilot import SunnypilotParamFlags +from opendbc.sunnypilot.mads_base import MadsCarStateBase +from opendbc.can.parser import CANParser + +MadsDataSP = namedtuple("MadsDataSP", + ["enable_mads", "paused", "lkas_disabled"]) + +ButtonType = structs.CarState.ButtonEvent.Type + + +class MadsCarController: + def __init__(self): + super().__init__() + self.mads = MadsDataSP(False, False, False) + + @staticmethod + def create_lkas_heartbit(packer, lkas_heartbit, mads): + # LKAS_HEARTBIT (0x2D9) LKAS heartbeat + values = {s: lkas_heartbit[s] for s in [ + "LKAS_DISABLED", + "AUTO_HIGH_BEAM", + "FORWARD_1", + "FORWARD_2", + "FORWARD_3", + ]} + + if mads.enable_mads: + values["LKAS_DISABLED"] = 1 if mads.lkas_disabled else 0 + + return packer.make_can_msg("LKAS_HEARTBIT", 0, values) + + @staticmethod + def mads_status_update(CC: structs.CarControl, CS) -> MadsDataSP: + enable_mads = CC.sunnypilotParams & SunnypilotParamFlags.ENABLE_MADS + paused = CC.madsEnabled and not CC.latActive + + if any(be.type == ButtonType.lkas and be.pressed for be in CS.out.buttonEvents): + CS.lkas_disabled = not CS.lkas_disabled + + return MadsDataSP(enable_mads, paused, CS.lkas_disabled) + + def update(self, CC: structs.CarControl, CS) -> None: + self.mads = self.mads_status_update(CC, CS) + + +class MadsCarState(MadsCarStateBase): + def __init__(self, CP: structs.CarParams): + super().__init__(CP) + self.lkas_heartbit = 0 + + self.init_lkas_disabled = False + self.lkas_disabled = False + + @staticmethod + def get_parser(CP, pt_messages, cam_messages) -> None: + if CP.carFingerprint in RAM_CARS: + pt_messages += [ + ("Center_Stack_1", 1), + ("Center_Stack_2", 1), + ] + else: + pt_messages.append(("TRACTION_BUTTON", 1)) + cam_messages.append(("LKAS_HEARTBIT", 1)) + + def get_lkas_button(self, cp, cp_cam): + if self.CP.carFingerprint in RAM_CARS: + lkas_button = cp.vl["Center_Stack_1"]["LKAS_Button"] or cp.vl["Center_Stack_2"]["LKAS_Button"] + else: + lkas_button = cp.vl["TRACTION_BUTTON"]["TOGGLE_LKAS"] + self.lkas_heartbit = cp_cam.vl["LKAS_HEARTBIT"] + if not self.init_lkas_disabled: + self.lkas_disabled = cp_cam.vl["LKAS_HEARTBIT"]["LKAS_DISABLED"] + self.init_lkas_disabled = True + + return lkas_button + + def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None: + cp = can_parsers[Bus.pt] + cp_cam = can_parsers[Bus.cam] + + self.prev_lkas_button = self.lkas_button + self.lkas_button = self.get_lkas_button(cp, cp_cam) diff --git a/opendbc/sunnypilot/car/ford/__init__.py b/opendbc/sunnypilot/car/ford/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/opendbc/sunnypilot/car/ford/mads.py b/opendbc/sunnypilot/car/ford/mads.py new file mode 100644 index 00000000..20f35d9f --- /dev/null +++ b/opendbc/sunnypilot/car/ford/mads.py @@ -0,0 +1,43 @@ +""" +The MIT License + +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + +Last updated: July 29, 2024 +""" + +from enum import StrEnum + +from opendbc.car import Bus,structs + +from opendbc.sunnypilot.mads_base import MadsCarStateBase +from opendbc.can.parser import CANParser + + +class MadsCarState(MadsCarStateBase): + def __init__(self, CP: structs.CarParams): + super().__init__(CP) + + def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None: + cp = can_parsers[Bus.pt] + + self.prev_lkas_button = self.lkas_button + self.lkas_button = cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"] diff --git a/opendbc/sunnypilot/car/hyundai/mads.py b/opendbc/sunnypilot/car/hyundai/mads.py new file mode 100644 index 00000000..aa9d4613 --- /dev/null +++ b/opendbc/sunnypilot/car/hyundai/mads.py @@ -0,0 +1,137 @@ +""" +The MIT License + +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + +Last updated: July 29, 2024 +""" + +from enum import StrEnum +from collections import namedtuple + +from opendbc.car import Bus, DT_CTRL, structs +from opendbc.car.hyundai.values import CAR + +from opendbc.sunnypilot import SunnypilotParamFlags +from opendbc.car.hyundai.values import HyundaiFlags +from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP +from opendbc.sunnypilot.mads_base import MadsCarStateBase +from opendbc.can.parser import CANParser + +ButtonType = structs.CarState.ButtonEvent.Type + +MadsDataSP = namedtuple("MadsDataSP", + ["enable_mads", "lat_active", "disengaging", "paused"]) + + +class MadsCarController: + def __init__(self): + super().__init__() + self.mads = MadsDataSP(False, False, False, False) + + self.lat_disengage_blink = 0 + self.lat_disengage_init = False + self.prev_lat_active = False + + self.lkas_icon = 0 + self.lfa_icon = 0 + + # display LFA "white_wheel" and LKAS "White car + lanes" when not CC.latActive + def mads_status_update(self, CC: structs.CarControl, frame: int) -> MadsDataSP: + enable_mads = CC.sunnypilotParams & SunnypilotParamFlags.ENABLE_MADS + + if CC.latActive: + self.lat_disengage_init = False + elif self.prev_lat_active: + self.lat_disengage_init = True + + if not self.lat_disengage_init: + self.lat_disengage_blink = frame + + paused = CC.madsEnabled and not CC.latActive + disengaging = (frame - self.lat_disengage_blink) * DT_CTRL < 1.0 if self.lat_disengage_init else False + + self.prev_lat_active = CC.latActive + + return MadsDataSP(enable_mads, CC.latActive, disengaging, paused) + + def create_lkas_icon(self, CP: structs.CarParams, enabled: bool) -> int: + if self.mads.enable_mads: + lkas_icon = 2 if self.mads.lat_active else 3 if self.mads.disengaging else 1 + else: + lkas_icon = 2 if enabled else 1 + + # Override common signals for KIA_OPTIMA_G4 and KIA_OPTIMA_G4_FL + if CP.carFingerprint in (CAR.KIA_OPTIMA_G4, CAR.KIA_OPTIMA_G4_FL): + lkas_icon = 3 if (self.mads.lat_active if self.mads.enable_mads else enabled) else 1 + + return lkas_icon + + def create_lfa_icon(self, enabled: bool) -> int: + if self.mads.enable_mads: + lfa_icon = 2 if self.mads.lat_active else 3 if self.mads.disengaging else 1 if self.mads.paused else 0 + else: + lfa_icon = 2 if enabled else 0 + + return lfa_icon + + def update(self, CP: structs.CarParams, CC: structs.CarControl, frame: int) -> None: + self.mads = self.mads_status_update(CC, frame) + self.lkas_icon = self.create_lkas_icon(CP, CC.enabled) + self.lfa_icon = self.create_lfa_icon(CC.enabled) + + +class MadsCarState(MadsCarStateBase): + def __init__(self, CP: structs.CarParams): + super().__init__(CP) + self.main_cruise_enabled: bool = False + self.cruise_btns_msg_canfd = None + + @staticmethod + def get_parser(CP, pt_messages) -> None: + if CP.sunnypilotFlags & HyundaiFlagsSP.HAS_LFA_BUTTON: + pt_messages.append(("BCM_PO_11", 50)) + + def get_main_cruise(self, ret: structs.CarState) -> bool: + if any(be.type == ButtonType.mainCruise and be.pressed for be in ret.buttonEvents) and \ + (self.CP.sunnypilotFlags & HyundaiFlagsSP.LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE): + self.main_cruise_enabled = not self.main_cruise_enabled + + return self.main_cruise_enabled if ret.cruiseState.available else False + + def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None: + cp = can_parsers[Bus.pt] + + self.prev_lkas_button = self.lkas_button + if self.CP.sunnypilotFlags & HyundaiFlagsSP.HAS_LFA_BUTTON: + self.lkas_button = cp.vl["BCM_PO_11"]["LFA_Pressed"] + + def update_mads_canfd(self, ret, can_parsers) -> None: + cp = can_parsers[Bus.pt] + cp_cam = can_parsers[Bus.cam] + + if not self.CP.openpilotLongitudinalControl: + cp_cruise_info = cp_cam if self.CP.flags & HyundaiFlags.CANFD_CAMERA_SCC else cp + ret.cruiseState.available = cp_cruise_info.vl["SCC_CONTROL"]["MainMode_ACC"] == 1 + + self.prev_lkas_button = self.lkas_button + lfa_button = "LFA_BTN" if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS else "LKAS_BTN" + self.lkas_button = cp.vl[self.cruise_btns_msg_canfd][lfa_button] diff --git a/opendbc/sunnypilot/car/hyundai/values.py b/opendbc/sunnypilot/car/hyundai/values.py index 9fd3f980..e7db910f 100644 --- a/opendbc/sunnypilot/car/hyundai/values.py +++ b/opendbc/sunnypilot/car/hyundai/values.py @@ -1,3 +1,29 @@ +""" +The MIT License + +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + +Last updated: July 29, 2024 +""" + from enum import IntFlag @@ -6,3 +32,5 @@ class HyundaiFlagsSP(IntFlag): Flags for Hyundai specific quirks within sunnypilot. """ ENHANCED_SCC = 1 + HAS_LFA_BUTTON = 2 + LONGITUDINAL_MAIN_CRUISE_TOGGLEABLE = 2 ** 2 diff --git a/opendbc/sunnypilot/car/subaru/__init__.py b/opendbc/sunnypilot/car/subaru/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/opendbc/sunnypilot/car/subaru/mads.py b/opendbc/sunnypilot/car/subaru/mads.py new file mode 100644 index 00000000..389d5ad8 --- /dev/null +++ b/opendbc/sunnypilot/car/subaru/mads.py @@ -0,0 +1,67 @@ +""" +The MIT License + +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + +Last updated: July 29, 2024 +""" + +from enum import StrEnum +from opendbc.car import Bus, structs + +from opendbc.car.subaru.values import SubaruFlags +from opendbc.sunnypilot.mads_base import MadsCarStateBase +from opendbc.can.parser import CANParser + +ButtonType = structs.CarState.ButtonEvent.Type + + +class MadsCarState(MadsCarStateBase): + def __init__(self, CP: structs.CarParams): + super().__init__(CP) + + @staticmethod + def create_lkas_button_events(cur_btn: int, prev_btn: int, + buttons_dict: dict[int, structs.CarState.ButtonEvent.Type]) -> list[structs.CarState.ButtonEvent]: + events: list[structs.CarState.ButtonEvent] = [] + + if cur_btn == prev_btn: + return events + + state_changes = [ + {"pressed": prev_btn != cur_btn and cur_btn != 2 and not (prev_btn == 2 and cur_btn == 1)}, + {"pressed": prev_btn != cur_btn and cur_btn == 2 and cur_btn != 1}, + ] + + for change in state_changes: + if change["pressed"]: + events.append(structs.CarState.ButtonEvent(pressed=change["pressed"], + type=buttons_dict.get(cur_btn, ButtonType.unknown))) + return events + + def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None: + cp_cam = can_parsers[Bus.cam] + + self.prev_lkas_button = self.lkas_button + if not self.CP.flags & SubaruFlags.PREGLOBAL: + self.lkas_button = cp_cam.vl["ES_LKAS_State"]["LKAS_Dash_State"] + + ret.buttonEvents = self.create_lkas_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas}) diff --git a/opendbc/sunnypilot/car/toyota/__init__.py b/opendbc/sunnypilot/car/toyota/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/opendbc/sunnypilot/car/toyota/mads.py b/opendbc/sunnypilot/car/toyota/mads.py new file mode 100644 index 00000000..b8efcdb2 --- /dev/null +++ b/opendbc/sunnypilot/car/toyota/mads.py @@ -0,0 +1,80 @@ +""" +The MIT License + +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + +Last updated: July 29, 2024 +""" + + +from enum import StrEnum + +from opendbc.car import Bus, structs + +from opendbc.car.toyota.values import CAR +from opendbc.sunnypilot.mads_base import MadsCarStateBase +from opendbc.can.parser import CANParser + +ButtonType = structs.CarState.ButtonEvent.Type + + +class MadsCarState(MadsCarStateBase): + distance_button_events: list[structs.CarState.ButtonEvent] + lkas_button_events: list[structs.CarState.ButtonEvent] + + def __init__(self, CP: structs.CarParams): + super().__init__(CP) + self.distance_button_events = [] + self.lkas_button_events = [] + + @staticmethod + def get_lkas_button(cp_cam): + lkas_button = cp_cam.vl["LKAS_HUD"]["LKAS_STATUS"] + + return lkas_button + + @staticmethod + def create_lkas_button_events(cur_btn: int, prev_btn: int, + buttons_dict: dict[int, structs.CarState.ButtonEvent.Type]) -> list[structs.CarState.ButtonEvent]: + events: list[structs.CarState.ButtonEvent] = [] + + if cur_btn == prev_btn: + return events + + state_changes = [ + {"pressed": bool(not prev_btn and cur_btn)}, + {"pressed": bool(prev_btn == 1 and not cur_btn)}, + ] + + for change in state_changes: + if change["pressed"]: + events.append(structs.CarState.ButtonEvent(pressed=change["pressed"], + type=buttons_dict.get(cur_btn, ButtonType.unknown))) + return events + + def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None: + cp_cam = can_parsers[Bus.cam] + + self.prev_lkas_button = self.lkas_button + if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V: + self.lkas_button = self.get_lkas_button(cp_cam) + + self.lkas_button_events = self.create_lkas_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas}) diff --git a/opendbc/sunnypilot/mads_base.py b/opendbc/sunnypilot/mads_base.py new file mode 100644 index 00000000..842c58e0 --- /dev/null +++ b/opendbc/sunnypilot/mads_base.py @@ -0,0 +1,43 @@ +""" +The MIT License + +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + +Last updated: July 29, 2024 +""" + +from abc import abstractmethod, ABC +from enum import StrEnum + +from opendbc.car import structs +from opendbc.can.parser import CANParser + + +class MadsCarStateBase(ABC): + def __init__(self, CP: structs.CarParams): + self.CP = CP + + self.lkas_button = 0 + self.prev_lkas_button = 0 + + @abstractmethod + def update_mads(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None: + pass