From dd5ff7e1d32b84360fd4bdc1147970aa2a8068a5 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 29 Sep 2024 11:20:30 -0400 Subject: [PATCH] fix --- cereal/car.capnp | 2 +- opendbc_repo | 2 +- panda | 2 +- selfdrive/car/card.py | 9 +++ selfdrive/pandad/panda.cc | 4 +- selfdrive/pandad/panda.h | 2 +- selfdrive/pandad/pandad.cc | 5 +- sunnypilot/mads/mads.py | 13 ++-- sunnypilot/mads/state.py | 4 +- sunnypilot/selfdrive/__init__.py | 0 sunnypilot/selfdrive/car/__init__.py | 0 .../selfdrive/car/hyundai/carcontroller.py | 38 +++++++++++ sunnypilot/selfdrive/car/interfaces.py | 64 +++++++++++++++++++ tools/cabana/streams/pandastream.cc | 2 +- 14 files changed, 129 insertions(+), 18 deletions(-) create mode 100644 sunnypilot/selfdrive/__init__.py create mode 100644 sunnypilot/selfdrive/car/__init__.py create mode 100644 sunnypilot/selfdrive/car/hyundai/carcontroller.py create mode 100644 sunnypilot/selfdrive/car/interfaces.py diff --git a/cereal/car.capnp b/cereal/car.capnp index 2c18c7c617..a6a10c871d 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -519,7 +519,7 @@ struct CarParams { safetyModel @0 :SafetyModel; safetyParam @3 :UInt16; safetyParamDEPRECATED @1 :Int16; - safetyParam2DEPRECATED @2 :UInt32; + spFlags @2 :UInt32; # flags for car specific quirks in sunnypilot } struct LateralParams { diff --git a/opendbc_repo b/opendbc_repo index c8beb979ec..a0722dad3e 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit c8beb979ecd675adeae0c35df7ba4b71f90685d6 +Subproject commit a0722dad3e5e8716d2b0b8902c86fd92a68bf80c diff --git a/panda b/panda index 035c78a549..c3b7f821cb 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 035c78a549aebbfb9787f94644d641b99766bde9 +Subproject commit c3b7f821cb4b19a291f946a7830f90c532725afa diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index f8058efc97..f0cc69aef7 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -155,6 +155,10 @@ class Car: # card is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) + data_services = list(self.sm.data.keys()) + ['selfdriveStateSP'] + self.sm = messaging.SubMaster(data_services, poll='selfdriveStateSP') + self.mads_enabled_toggle = True # TODO-SP: Apply with toggle + def state_update(self) -> tuple[car.CarState, structs.RadarData | None]: """carState update loop, driven by can""" @@ -251,6 +255,11 @@ class Car: # signal pandad to switch to car safety mode self.params.put_bool_nonblocking("ControlsReady", True) + if self.mads_enabled_toggle: + self.CI.mads_enabled = self.sm['selfdriveStateSP'].mads.enabled + else: + self.CI.mads_enabled = self.sm['carControl'].enabled + if self.sm.all_alive(['carControl']): # send car controls over can now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9) diff --git a/selfdrive/pandad/panda.cc b/selfdrive/pandad/panda.cc index 5372e54aef..0209e24d4d 100644 --- a/selfdrive/pandad/panda.cc +++ b/selfdrive/pandad/panda.cc @@ -137,8 +137,8 @@ void Panda::enable_deepsleep() { handle->control_write(0xfb, 0, 0); } -void Panda::send_heartbeat(bool engaged) { - handle->control_write(0xf3, engaged, 0); +void Panda::send_heartbeat(bool engaged, bool mads_engaged) { + handle->control_write(0xf3, engaged, mads_engaged); } void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) { diff --git a/selfdrive/pandad/panda.h b/selfdrive/pandad/panda.h index af31c2316e..377ae954bc 100644 --- a/selfdrive/pandad/panda.h +++ b/selfdrive/pandad/panda.h @@ -75,7 +75,7 @@ public: std::optional get_serial(); void set_power_saving(bool power_saving); void enable_deepsleep(); - void send_heartbeat(bool engaged); + void send_heartbeat(bool engaged, bool mads_engaged); void set_can_speed_kbps(uint16_t bus, uint16_t speed); void set_data_speed_kbps(uint16_t bus, uint16_t speed); void set_canfd_non_iso(uint16_t bus, bool non_iso); diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index b0b10fe315..fc058d547a 100644 --- a/selfdrive/pandad/pandad.cc +++ b/selfdrive/pandad/pandad.cc @@ -313,7 +313,7 @@ void send_peripheral_state(Panda *panda, PubMaster *pm) { } void process_panda_state(std::vector &pandas, PubMaster *pm, bool spoofing_started) { - static SubMaster sm({"selfdriveState"}); + static SubMaster sm({"selfdriveState", "selfdriveStateSP"}); std::vector connected_serials; for (Panda *p : pandas) { @@ -352,8 +352,9 @@ void process_panda_state(std::vector &pandas, PubMaster *pm, bool spoof sm.update(0); const bool engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled(); + const bool mads_engaged = sm.allAliveAndValid({"selfdriveStateSP"}) && sm["selfdriveStateSP"].getSelfdriveStateSP().getMads().getEnabled(); for (const auto &panda : pandas) { - panda->send_heartbeat(engaged); + panda->send_heartbeat(engaged, mads_engaged); } } } diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index b1ca55128c..fdc6036d92 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -28,14 +28,13 @@ class ModifiedAssistDrivingSystem: self.main_enabled_toggle = True # TODO-SP: Apply with toggle self.disengage_lateral_on_brake_toggle = False # TODO-SP: Apply with toggle - self.mads_enabled = False + self.available = False self.mads_alt_button_enabled = False def set_alternative_experience(self, alt_experience: int = 0): if self.enabled_toggle: - if self.disengage_lateral_on_brake_toggle: - alt_experience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS - else: + alt_experience |= ALTERNATIVE_EXPERIENCE.ENABLE_MADS + if not self.disengage_lateral_on_brake_toggle: alt_experience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_LATERAL_ON_BRAKE return alt_experience @@ -73,7 +72,7 @@ class ModifiedAssistDrivingSystem: self.selfdrive.events.add(EventName.silentPedalPressed) if not CS.brakePressed and not CS.brakeHoldActive and not CS.parkingBrake and not CS.regenBraking: - if self.current_state == State.paused and self.mads_enabled: + if self.current_state == State.paused and self.available: self.selfdrive.events.add(EventName.silentButtonEnable) for be in CS.buttonEvents: @@ -81,7 +80,7 @@ class ModifiedAssistDrivingSystem: if self.selfdrive.enabled: self.selfdrive.events.add(EventName.manualLongitudinalRequired) if be.type == ButtonType.altButton1 and be.pressed: - if not self.mads_enabled: + if not self.available: if not self.selfdrive.enabled: self.selfdrive.events.add(EventName.buttonCancel) else: @@ -94,7 +93,7 @@ class ModifiedAssistDrivingSystem: if not self.enabled_toggle: return - self.mads_enabled = self.update_availability(CS) + self.available = self.update_availability(CS) self.update_events(CS) diff --git a/sunnypilot/mads/state.py b/sunnypilot/mads/state.py index 1d13ee5632..f18196da14 100644 --- a/sunnypilot/mads/state.py +++ b/sunnypilot/mads/state.py @@ -39,8 +39,8 @@ class StateMachineBase(ABC): elif self.state == State.disabled: self.handle(events) - enabled = self.state in ENABLED_STATES and self.mads.mads_enabled - active = self.state in ACTIVE_STATES and self.mads.mads_enabled + enabled = self.state in ENABLED_STATES and self.mads.available + active = self.state in ACTIVE_STATES and self.mads.available if active: self.add_current_alert_types(ET.WARNING) diff --git a/sunnypilot/selfdrive/__init__.py b/sunnypilot/selfdrive/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/__init__.py b/sunnypilot/selfdrive/car/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/car/hyundai/carcontroller.py b/sunnypilot/selfdrive/car/hyundai/carcontroller.py new file mode 100644 index 0000000000..1517803af2 --- /dev/null +++ b/sunnypilot/selfdrive/car/hyundai/carcontroller.py @@ -0,0 +1,38 @@ +from collections import namedtuple + +from opendbc.car import DT_CTRL, structs +from opendbc.car.interfaces import CarStateBase + +MadsDataSP = namedtuple("MadsDataSP", + ["enabled_toggle", "lat_active", "disengaging", "lfa_icon"]) + + +class CarControllerSP: + def __init__(self, car_controller): + self.CC = car_controller + + self.lat_disengage_blink = 0 + self.lat_disengage_init = False + self.prev_lat_active = False + + # display LFA "white_wheel" and LKAS "White car + lanes" when not CC.latActive + def mads_status_update(self, CS: CarStateBase, CC: structs.CarControl) -> MadsDataSP: + 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 = self.CC.frame + + paused = CS.mads_enabled and not CC.latActive + disengaging = (self.CC.frame - self.lat_disengage_blink) * DT_CTRL < 1.0 if self.lat_disengage_init else False + + if CS.mads_enabled_toggle: + lfa_icon = 2 if CC.latActive else 3 if disengaging else 1 if paused else 0 + else: + lfa_icon = 2 if CC.enabled else 0 + + self.prev_lat_active = CC.latActive + + return MadsDataSP(CS.mads_enabled_toggle, CC.latActive, disengaging, lfa_icon) diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py new file mode 100644 index 0000000000..852a7b561c --- /dev/null +++ b/sunnypilot/selfdrive/car/interfaces.py @@ -0,0 +1,64 @@ +from abc import ABC + +from opendbc.car import structs + +ButtonType = structs.CarState.ButtonEvent.Type + + +class CarInterfaceBaseSP(ABC): + def __init__(self, CP: structs.CarParams, CarController, CarState): + self.CP = CP + + self.CS: CarStateBaseSP = CarState(CP) + self.cp = self.CS.get_can_parser(CP) + self.cp_cam = self.CS.get_cam_can_parser(CP) + self.cp_adas = self.CS.get_adas_can_parser(CP) + self.cp_body = self.CS.get_body_can_parser(CP) + self.cp_loopback = self.CS.get_loopback_can_parser(CP) + self.can_parsers = (self.cp, self.cp_cam, self.cp_adas, self.cp_body, self.cp_loopback) + + dbc_name = "" if self.cp is None else self.cp.dbc_name + self.CC: CarControllerBaseSP = CarController(dbc_name, CP) + + self.mads_enabled_toggle = False + self.mads_enabled = False + + def update_prevs(self): + self.CS.mads_enabled_toggle = self.mads_enabled_toggle + self.CS.mads_enabled = self.mads_enabled + + +class CarStateBaseSP(ABC): + def __init__(self, CP: structs.CarParams): + self.CP = CP + + self.alt_button = 0 + self.mads_enabled_toggle = False + self.mads_enabled = False + self.button_events: list[structs.CarState.ButtonEvent] = [] + + @staticmethod + def get_can_parser(CP): + return None + + @staticmethod + def get_cam_can_parser(CP): + return None + + @staticmethod + def get_adas_can_parser(CP): + return None + + @staticmethod + def get_body_can_parser(CP): + return None + + @staticmethod + def get_loopback_can_parser(CP): + return None + + +class CarControllerBaseSP(ABC): + def __init__(self, dbc_name: str, CP: structs.CarParams): + self.CP = CP + self.frame = 0 diff --git a/tools/cabana/streams/pandastream.cc b/tools/cabana/streams/pandastream.cc index b2a006b22f..e66d72e59d 100644 --- a/tools/cabana/streams/pandastream.cc +++ b/tools/cabana/streams/pandastream.cc @@ -72,7 +72,7 @@ void PandaStream::streamThread() { handleEvent(capnp::messageToFlatArray(msg)); - panda->send_heartbeat(false); + panda->send_heartbeat(false, false); } }