controlsd: verify unsafe mode consistency (#23563)

* add unsafe mode check to controlsd

bump submodules

forgot to add to health_t struct

bump

ahh forgot to set it!

bump

bump

clean up

clean up

bump

* fix ordering

* move to end

* unsigned

Co-authored-by: Willem Melching <willem.melching@gmail.com>
old-commit-hash: 31d147dca2

selfdrive/boardd/boardd.cc

@@ -161,7 +161,8 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
   int safety_param;
 
   auto safety_configs = car_params.getSafetyConfigs();
-  for (uint32_t i=0; i<pandas.size(); i++) {
+  uint16_t unsafe_mode = car_params.getUnsafeMode();
+  for (uint32_t i = 0; i < pandas.size(); i++) {
     auto panda = pandas[i];
 
     if (safety_configs.size() > i) {
@@ -173,9 +174,8 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
       safety_param = 0;
     }
 
-    LOGW("panda %d: setting safety model: %d with param %d", i, (int)safety_model, safety_param);
-
-    panda->set_unsafe_mode(0);  // see safety_declarations.h for allowed values
+    LOGW("panda %d: setting safety model: %d, param: %d, unsafe mode: %d", i, (int)safety_model, safety_param, unsafe_mode);
+    panda->set_unsafe_mode(unsafe_mode);
     panda->set_safety_model(safety_model, safety_param);
   }
 
@@ -315,7 +315,7 @@ bool send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool s
     pandaStates.push_back(pandaState);
   }
 
-  for (uint32_t i=0; i<pandas.size(); i++) {
+  for (uint32_t i = 0; i < pandas.size(); i++) {
     auto panda = pandas[i];
     const auto &pandaState = pandaStates[i];
 
@@ -356,6 +356,7 @@ bool send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool s
     ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status));
     ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled));
     ps.setHeartbeatLost((bool)(pandaState.heartbeat_lost));
+    ps.setUnsafeMode(pandaState.unsafe_mode);
     ps.setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status));
 
     // Convert faults bitset to capnp list

selfdrive/boardd/panda.h

@@ -45,6 +45,7 @@ struct __attribute__((packed)) health_t {
   uint8_t fault_status;
   uint8_t power_save_enabled;
   uint8_t heartbeat_lost;
+  uint16_t unsafe_mode;
 };
 
 struct __attribute__((packed)) can_header {

selfdrive/car/interfaces.py

@@ -72,6 +72,7 @@ class CarInterfaceBase(ABC):
   def get_std_params(candidate, fingerprint):
     ret = car.CarParams.new_message()
     ret.carFingerprint = candidate
+    ret.unsafeMode = 0  # see safety_declarations.h for allowed values
 
     # standard ALC params
     ret.steerControlType = car.CarParams.SteerControlType.torque

selfdrive/controls/controlsd.py

@@ -253,7 +253,9 @@ class Controls:
     for i, pandaState in enumerate(self.sm['pandaStates']):
       # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
       if i < len(self.CP.safetyConfigs):
-        safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam
+        safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \
+                          pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \
+                          pandaState.unsafeMode != self.CP.unsafeMode
       else:
         safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES