move to safety_mads

board/safety.h

@@ -3,7 +3,7 @@
 #include "safety_declarations.h"
 #include "can.h"
 
-#include "sunnypilot/safety_common.h"
+#include "sunnypilot/safety_mads.h"
 
 // include the safety policies.
 #include "safety/safety_defaults.h"
@@ -83,16 +83,6 @@ bool acc_main_on = false;  // referred to as "ACC off" in ISO 15622:2018
 int cruise_button_prev = 0;
 bool safety_rx_checks_invalid = false;
 
-// mads
-bool enable_mads = false;
-bool disengage_lateral_on_brake = false;
-bool disengaged_from_brakes = false;
-bool controls_allowed_lat = false;
-bool lkas_button = false;
-bool acc_main_on_prev = false;
-bool main_button_prev = false;
-uint32_t acc_main_on_mismatches = 0;
-
 // for safety modes with torque steering control
 int desired_torque_last = 0;       // last desired steer torque
 int rt_torque_last = 0;            // last desired torque for real time check

board/safety/safety_hyundai.h

@@ -210,7 +210,7 @@ static bool hyundai_tx_hook(const CANPacket_t *to_send) {
 
   if (addr == 0x420) {
     bool acc_main_on_tx = GET_BIT(to_send, 0U);
-    hyundai_common_reset_acc_main(acc_main_on_tx);
+    reset_acc_main(acc_main_on_tx);
   }
 
   // ACCEL: safety check

board/safety/safety_hyundai_canfd.h

@@ -204,7 +204,7 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send) {
     }
 
     bool acc_main_on_tx = GET_BIT(to_send, 66U);
-    hyundai_common_reset_acc_main(acc_main_on_tx);
+    reset_acc_main(acc_main_on_tx);
   }
 
   return tx;

board/safety/safety_hyundai_common.h

@@ -137,15 +137,3 @@ uint32_t hyundai_common_canfd_compute_checksum(const CANPacket_t *to_push) {
 
   return crc;
 }
-
-void hyundai_common_reset_acc_main(const bool acc_main_on_tx) {
-  if (acc_main_on && !acc_main_on_tx) {
-    acc_main_on_mismatches += 1U;
-    if (acc_main_on_mismatches >= 25U) {
-      acc_main_on = acc_main_on_tx;
-      mads_check_acc_main();
-    }
-  } else {
-    acc_main_on_mismatches = 0U;
-  }
-}

board/safety_declarations.h

@@ -207,23 +207,6 @@ extern bool acc_main_on; // referred to as "ACC off" in ISO 15622:2018
 extern int cruise_button_prev;
 extern bool safety_rx_checks_invalid;
 
-// mads
-void mads_set_state(bool state);
-void mads_check_acc_main(void);
-void mads_check_lkas_button(void);
-void mads_exit_controls(void);
-void mads_resume_controls(void);
-void check_braking_condition(bool state, bool state_prev);
-
-extern bool enable_mads;
-extern bool disengage_lateral_on_brake;
-extern bool disengaged_from_brakes;
-extern bool controls_allowed_lat;
-extern bool lkas_button;
-extern bool acc_main_on_prev;
-extern bool main_button_prev;
-extern uint32_t acc_main_on_mismatches;
-
 // for safety modes with torque steering control
 extern int desired_torque_last;       // last desired steer torque
 extern int rt_torque_last;            // last desired torque for real time check
@@ -261,16 +244,6 @@ extern struct sample_t angle_meas;         // last 6 steer angles/curvatures
 // This flag allows AEB to be commanded from openpilot.
 #define ALT_EXP_ALLOW_AEB 16
 
-// Enable the ability to enable sunnypilot Automatic Lane Centering and ACC/SCC independently of each other. This
-// will enable MADS and allow other features to be used.
-// Enable the ability to re-engage sunnypilot Automatic Lane Centering only (NOT ACC/SCC) on brake release while MADS
-// is enabled.
-#define ALT_EXP_ENABLE_MADS 1024
-
-// Enable the ability to disable disengaging lateral control on brake press while MADS is enabled.
-// The feature must be gated behind this flag per geohot's comment on the comma community Discord server.
-#define ALT_EXP_DISABLE_DISENGAGE_LATERAL_ON_BRAKE 2048
-
 extern int alternative_experience;
 
 // time since safety mode has been changed

board/sunnypilot/safety_mads.h

@@ -0,0 +1,103 @@
+#pragma once
+
+void mads_set_state(bool state);
+void mads_check_acc_main(void);
+void mads_check_lkas_button(void);
+void mads_exit_controls(void);
+void mads_resume_controls(void);
+void check_braking_condition(bool state, bool state_prev);
+
+extern bool enable_mads;
+bool enable_mads = false;
+
+extern bool disengage_lateral_on_brake;
+bool disengage_lateral_on_brake = false;
+
+extern bool disengaged_from_brakes;
+bool disengaged_from_brakes = false;
+
+extern bool controls_allowed_lat;
+bool controls_allowed_lat = false;
+
+extern bool lkas_button;
+bool lkas_button = false;
+
+extern bool acc_main_on_prev;
+bool acc_main_on_prev = false;
+
+extern bool main_button_prev;
+bool main_button_prev = false;
+
+extern uint32_t acc_main_on_mismatches;
+uint32_t acc_main_on_mismatches = 0;
+
+// Enable the ability to enable sunnypilot Automatic Lane Centering and ACC/SCC independently of each other. This
+// will enable MADS and allow other features to be used.
+// Enable the ability to re-engage sunnypilot Automatic Lane Centering only (NOT ACC/SCC) on brake release while MADS
+// is enabled.
+#define ALT_EXP_ENABLE_MADS 1024
+
+// Enable the ability to disable disengaging lateral control on brake press while MADS is enabled.
+// The feature must be gated behind this flag per geohot's comment on the comma community Discord server.
+#define ALT_EXP_DISABLE_DISENGAGE_LATERAL_ON_BRAKE 2048
+
+void mads_set_state(bool state) {
+  controls_allowed_lat = state;
+  disengaged_from_brakes = state;
+}
+
+void mads_check_acc_main(void) {
+  if (acc_main_on && enable_mads) {
+    controls_allowed_lat = true;
+  }
+
+  if (!acc_main_on && acc_main_on_prev) {
+    controls_allowed = false;
+    mads_set_state(false);
+  }
+  acc_main_on_prev = acc_main_on;
+}
+
+void mads_check_lkas_button(void) {
+  if (lkas_button && enable_mads) {
+    controls_allowed_lat = true;
+  }
+}
+
+void mads_exit_controls(void) {
+  if (controls_allowed_lat) {
+    disengaged_from_brakes = true;
+    controls_allowed_lat = false;
+  }
+}
+
+void mads_resume_controls(void) {
+  if (disengaged_from_brakes) {
+    controls_allowed_lat = true;
+    disengaged_from_brakes = false;
+  }
+}
+
+void check_braking_condition(bool state, bool state_prev) {
+  if (state && (!state_prev || vehicle_moving)) {
+    controls_allowed = false;
+    if (disengage_lateral_on_brake) {
+      mads_exit_controls();
+    }
+  } else if (!state && disengage_lateral_on_brake) {
+    mads_resume_controls();
+  } else {
+  }
+}
+
+void reset_acc_main(const bool acc_main_on_tx) {
+  if (acc_main_on && !acc_main_on_tx) {
+    acc_main_on_mismatches += 1U;
+    if (acc_main_on_mismatches >= 25U) {
+      acc_main_on = acc_main_on_tx;
+      mads_check_acc_main();
+    }
+  } else {
+    acc_main_on_mismatches = 0U;
+  }
+}