From c23f2dce2cc7193bc9e2ee05af7ed758f6789874 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 14 Apr 2026 16:28:13 -0400 Subject: [PATCH] MADS safety: enable heartbeat and lateral controls mismatch checks (#1801) * init * nah * rename * bump * bump --- cereal/log.capnp | 4 ++-- opendbc_repo | 2 +- panda | 2 +- selfdrive/pandad/pandad.cc | 15 ++++----------- sunnypilot/mads/mads.py | 17 +++++++++++++++++ tools/cabana/panda.cc | 4 ++-- tools/cabana/panda.h | 2 +- tools/sim/lib/simulated_car.py | 2 ++ 8 files changed, 30 insertions(+), 18 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index 7138a6178..cf91e017e 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -558,8 +558,8 @@ struct PandaState @0xa7649e2575e4591e { # these fields are not used by openpilot, but they're # reserved for forks building alternate experiences. - controlsAllowedRESERVED1 @38 :Bool; - controlsAllowedRESERVED2 @39 :Bool; + controlsAllowedLateral @38 :Bool; + controlsAllowedLongitudinal @39 :Bool; enum FaultStatus { none @0; diff --git a/opendbc_repo b/opendbc_repo index 427032a89..ded068839 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 427032a89a556b4d80b700749582f68807fe2443 +Subproject commit ded068839b7b84708f35b07ad207601f36652f4e diff --git a/panda b/panda index 6cd1972ec..c0cc96fba 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 6cd1972ecf31429abb55647cadeb8ab3e19a9141 +Subproject commit c0cc96fbad1d11445c34141d7626703fc13a9938 diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index 3d0a551d8..cafc3e122 100644 --- a/selfdrive/pandad/pandad.cc +++ b/selfdrive/pandad/pandad.cc @@ -21,8 +21,6 @@ #define CUTOFF_IL 400 #define SATURATE_IL 1000 -#define ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE 2048 - ExitHandler do_exit; bool check_connected(Panda *panda) { @@ -34,15 +32,8 @@ bool check_connected(Panda *panda) { } bool process_mads_heartbeat(SubMaster *sm) { - const int &alt_exp = (*sm)["carParams"].getCarParams().getAlternativeExperience(); - const bool disengage_lateral_on_brake = (alt_exp & ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE) != 0; - const auto &mads = (*sm)["selfdriveStateSP"].getSelfdriveStateSP().getMads(); - const bool heartbeat_type = disengage_lateral_on_brake ? mads.getActive() : mads.getEnabled(); - - const bool engaged = sm->allAliveAndValid({"selfdriveStateSP"}) && heartbeat_type; - - return engaged; + return sm->allAliveAndValid({"selfdriveStateSP"}) && mads.getEnabled(); } Panda *connect(std::string serial) { @@ -152,6 +143,8 @@ void fill_panda_state(cereal::PandaState::Builder &ps, cereal::PandaState::Panda ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f); ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f); ps.setSoundOutputLevel(health.sound_output_level_pkt); + ps.setControlsAllowedLateral(health.controls_allowed_lateral_pkt); + ps.setControlsAllowedLongitudinal(health.controls_allowed_longitudinal_pkt); } void fill_panda_can_state(cereal::PandaState::PandaCanState::Builder &cs, const can_health_t &can_health) { @@ -380,7 +373,7 @@ void pandad_run(Panda *panda) { Params params; RateKeeper rk("pandad", 100); - SubMaster sm({"selfdriveState", "selfdriveStateSP", "carParams"}); + SubMaster sm({"selfdriveState", "selfdriveStateSP"}); PubMaster pm({"can", "pandaStates", "peripheralState"}); PandaSafety panda_safety(panda); bool engaged = false; diff --git a/sunnypilot/mads/mads.py b/sunnypilot/mads/mads.py index 7eab55e6e..0c87da4a2 100644 --- a/sunnypilot/mads/mads.py +++ b/sunnypilot/mads/mads.py @@ -33,6 +33,7 @@ class ModularAssistiveDrivingSystem: self.enabled = False self.active = False self.available = False + self.lateral_mismatch_counter = 0 self.allow_always = False self.no_main_cruise = False self.selfdrive = selfdrive @@ -104,6 +105,17 @@ class ModularAssistiveDrivingSystem: self.events.remove(old_event) self.events_sp.add(new_event) + def data_sample(self): + # When the safety and selfdrived do not agree on controls_allowed_lateral + # we want to disengage sunnypilot. However the status from the panda goes through + # another socket other than the CAN messages and one can arrive earlier than the other. + # Therefore we allow a mismatch for two samples, then we trigger the disengagement. + if not self.active or self.selfdrive.enabled: + self.lateral_mismatch_counter = 0 + elif any(not ps.controlsAllowedLateral for ps in self.selfdrive.sm['pandaStates'] + if ps.safetyModel not in IGNORED_SAFETY_MODES): + self.lateral_mismatch_counter += 1 + def update_events(self, CS: structs.CarState): if not self.selfdrive.enabled and self.enabled: if CS.standstill: @@ -186,6 +198,9 @@ class ModularAssistiveDrivingSystem: if self.state_machine.state == State.paused: self.events_sp.add(EventNameSP.silentLkasEnable) + if self.lateral_mismatch_counter >= 200: + self.events_sp.add(EventNameSP.controlsMismatchLateral) + self.events.remove(EventName.pcmDisable) self.events.remove(EventName.buttonCancel) self.events.remove(EventName.pedalPressed) @@ -195,6 +210,8 @@ class ModularAssistiveDrivingSystem: if not self.enabled_toggle: return + self.data_sample() + self.update_events(CS) if not self.CP.passive and self.selfdrive.initialized: diff --git a/tools/cabana/panda.cc b/tools/cabana/panda.cc index 0612d6774..cf5354a50 100644 --- a/tools/cabana/panda.cc +++ b/tools/cabana/panda.cc @@ -106,8 +106,8 @@ cereal::PandaState::PandaType Panda::get_hw_type() { -void Panda::send_heartbeat(bool engaged) { - control_write(0xf3, engaged, 0); +void Panda::send_heartbeat(bool engaged, bool engaged_mads) { + control_write(0xf3, engaged, engaged_mads); } void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) { diff --git a/tools/cabana/panda.h b/tools/cabana/panda.h index d318c33f4..8b861a247 100644 --- a/tools/cabana/panda.h +++ b/tools/cabana/panda.h @@ -64,7 +64,7 @@ public: // Panda functionality cereal::PandaState::PandaType get_hw_type(); void set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param=0U); - void send_heartbeat(bool engaged); + void send_heartbeat(bool engaged, bool engaged_mads = false); void set_can_speed_kbps(uint16_t bus, uint16_t speed); void set_data_speed_kbps(uint16_t bus, uint16_t speed); bool can_receive(std::vector& out_vec); diff --git a/tools/sim/lib/simulated_car.py b/tools/sim/lib/simulated_car.py index 68ff3050d..1567c20ef 100644 --- a/tools/sim/lib/simulated_car.py +++ b/tools/sim/lib/simulated_car.py @@ -92,6 +92,8 @@ class SimulatedCar: 'ignitionLine': simulator_state.ignition, 'pandaType': "blackPanda", 'controlsAllowed': True, + 'controlsAllowedLateral': True, + 'controlsAllowedLongitudinal': True, 'safetyModel': 'hondaBosch', 'alternativeExperience': self.sm["carParams"].alternativeExperience, 'safetyParam': HondaSafetyFlags.RADARLESS.value | HondaSafetyFlags.BOSCH_LONG.value,