rename

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;

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;

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:

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) {

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<can_frame>& out_vec);

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,