Modular Assistive Driving System (MADS) (#40)

* alternative experience * safety init * fix * more update * not really * misra * Add Custom MIT License (#38) * brake check was not handled * revert * alt -> lkas * explicit checks * support toyota and ford * rename * hyundai can-fd support * only allow lkas if enabled * hyundai: main button handling * revert * hyundai: main button heartbeat * add logging for controls allowed lateral * fix panda safety * ford btn * toyota btn * fca btn * honda btn * mads safety tests * more tests * safety misra * safety mutation * misra * mutation experiment * fix * ford test main button * ford test lkas button * more ford test * hyundai lkas and main * more ford * hyundai canfd * rename * rename * cleaner * more fixes * more hyundai tests * no longer needed * thanks for tests! * more tests for lat * more explicit * make sure to reset * try this out * probably needed * move * misra * not needed * move to safety_mads * not really needed * remove * MADS: Refactor MADS safety with improved state management (pull request #46) Refactor MADS safety with improved state management This commit introduces a major refactoring of the MADS safety module, improving state management and control flow. Key changes include: Core Changes: - Introduced a MADSState struct to centralize state management - Removed global state variables in favor of structured state - Implemented button transition handling with explicit state tracking (PRESSED/RELEASED/NO_CHANGE) - Added state flags for button availability detection - Simplified lateral control permission logic Button Handling: - Separated main button and LKAS button state tracking - Added independent engagement states for each button - Improved button press detection across multiple platforms - Added support for main and LKAS buttons on Hyundai platforms - Modified ACC main state handling Testing: - Added comprehensive test coverage for MADS state transitions - Added new MADS-specific test base class for consistent testing across platforms - Added mutation testing for state management - Extended timeout for mutation tests from 5 to 8 minutes - Added extensive button press validation tests - Enhanced debugging output in replay drive tests The refactored code provides a more organized implementation of MADS safety features while maintaining compatibility with existing safety checks. * adding note * adding ford (WIP) * adding honda (WIP) * adding toyota (WIP) * adding chrysler (WIP) * Standardize Button State Handling Across Platforms Refactor button state handling by replacing integer constants with an enumerated `ButtonState` type and updating logic to improve readability and maintainability. This change affects button press detection in Ford, Honda, Hyundai, and Toyota safety modules and aligns them with a unified MADS button state approach. Enums provide a clearer understanding of button states and transitions, facilitating easier maintenance and future enhancements. * Disable LKAS button press logic in Honda and Toyota safety. The code for processing LKAS button presses has been commented out in both Honda and Toyota safety implementations. This change aims to investigate or temporarily halt the button press effects without removing the logic altogether. It will be important to test for any impacts this may have on vehicle control functionality. * Remove commented out code in toyota_rx_hook function This commit cleans up the toyota_rx_hook function by removing unnecessary commented-out code that checks for LKAS button presses on bus 2. This helps improve code readability and maintainability without altering the existing functionality. * GM, mazda, nissan, subaru (global & preglobal) * Honda LKAS * Revert "Remove commented out code in toyota_rx_hook function" This reverts commit d6b012c01a08118d91fad56c9f6ac2f92b671968. * Toyota, Subaru Global LKAS * nissan fix * gm fix * use speed msg to force rx * im bored * misra * subaru/toyota/honda * nope * attempt * go through all buttons * try nissan * more nissan * nissan tests passed! * subaru lkas test (not sure why it's not passing 2 and 3 values) * Improved code organization in safety_subaru.h and test_subaru.py This commit includes a minor restructuring in safety_subaru.h and test_subaru.py for better readability and flow. The condition check in safety_subaru.h for lkas_hud now has explicit parentheses. With regard to test_subaru.py, an unnecessary import was removed, and the sequence of steps in the test was reordered - now enabling mads and cleaning up mads_states happens before each subtest. * Refactor tests to use _speed_msg instead of _user_brake_msg. Updated the MADS safety tests to utilize the _speed_msg(0) function call in place of _user_brake_msg(False). * Reworking the tests a little for clarity * disabling lkas again on toyota temporarily * fix mads condition to engage * hyundai and honda good with new tests * Redoing more tests * update for safety tick ensuring mads control is exited while lagging * Updating tests for toyota * cleaning up tests on hkg * commenting out temp_debug for future use * revert * constants * cleanup * format! * match yota * Apply suggestions from code review * force * explicit checks * revert --------- Co-authored-by: Jason Wen <haibin.wen3@gmail.com>
2026-02-18 09:13:52 +08:00 · 2024-12-06 01:39:36 +01:00
parent 67b4fa2430
commit 4c2ccd7fe5
35 changed files with 899 additions and 10 deletions
--- a/.github/workflows/test.yaml
+++ b/.github/workflows/test.yaml
@@ -128,7 +128,7 @@ jobs:
      - name: Build Docker image
        run: eval "$BUILD"
      - name: Mutation tests
-        timeout-minutes: 5
+        timeout-minutes: 8
        run: ${{ env.RUN }} "GIT_REF=${{ github.event_name == 'push' && (github.ref == 'refs/heads/master' || github.ref == 'refs/heads/master-new') && github.event.before || 'origin/master' }} cd tests/safety && ./mutation.sh"

  static_analysis:
--- a/board/health.h
+++ b/board/health.h
@@ -13,6 +13,7 @@ struct __attribute__((packed)) health_t {
  uint8_t ignition_line_pkt;
  uint8_t ignition_can_pkt;
  uint8_t controls_allowed_pkt;
+  uint8_t controls_allowed_lat_pkt;
  uint8_t car_harness_status_pkt;
  uint8_t safety_mode_pkt;
  uint16_t safety_param_pkt;
--- a/board/main_comms.h
+++ b/board/main_comms.h
@@ -17,6 +17,7 @@ static int get_health_pkt(void *dat) {
  health->ignition_can_pkt = ignition_can;

  health->controls_allowed_pkt = controls_allowed;
+  health->controls_allowed_lat_pkt = mads_is_lateral_control_allowed_by_mads();
  health->safety_tx_blocked_pkt = safety_tx_blocked;
  health->safety_rx_invalid_pkt = safety_rx_invalid;
  health->tx_buffer_overflow_pkt = tx_buffer_overflow;
@@ -247,6 +248,9 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
      // you can only set this if you are in a non car safety mode
      if (!is_car_safety_mode(current_safety_mode)) {
        alternative_experience = req->param1;
+        bool mads_enabled = (alternative_experience & ALT_EXP_ENABLE_MADS) != 0;
+        bool disengage_lateral_on_brake = !(alternative_experience & ALT_EXP_DISABLE_DISENGAGE_LATERAL_ON_BRAKE);
+        mads_set_system_state(mads_enabled, disengage_lateral_on_brake);
      }
      break;
    // **** 0xe0: uart read
--- a/board/safety.h
+++ b/board/safety.h
@@ -3,6 +3,8 @@
 #include "safety_declarations.h"
 #include "can.h"

+#include "sunnypilot/safety_mads.h"
+
 // include the safety policies.
 #include "safety/safety_defaults.h"
 #include "safety/safety_honda.h"
@@ -111,6 +113,10 @@ uint16_t current_safety_param = 0;
 static const safety_hooks *current_hooks = &nooutput_hooks;
 safety_config current_safety_config;

+static bool is_lat_active(void) {
+  return controls_allowed || mads_is_lateral_control_allowed_by_mads();
+}
+
 static bool is_msg_valid(RxCheck addr_list[], int index) {
  bool valid = true;
  if (index != -1) {
@@ -301,6 +307,7 @@ void safety_tick(const safety_config *cfg) {
      cfg->rx_checks[i].status.lagging = lagging;
      if (lagging) {
        controls_allowed = false;
+        mads_exit_controls();
      }

      if (lagging || !is_msg_valid(cfg->rx_checks, i)) {
@@ -343,6 +350,7 @@ void generic_rx_checks(bool stock_ecu_detected) {
  if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && stock_ecu_detected) {
    relay_malfunction_set();
  }
+  mads_state_update(&vehicle_moving, &acc_main_on, brake_pressed || regen_braking, cruise_engaged_prev);
 }

 static void relay_malfunction_reset(void) {
@@ -601,7 +609,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
  bool violation = false;
  uint32_t ts = microsecond_timer_get();

-  if (controls_allowed) {
+  if (is_lat_active()) {
    // *** global torque limit check ***
    violation |= max_limit_check(desired_torque, limits.max_steer, -limits.max_steer);

@@ -628,7 +636,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
  }

  // no torque if controls is not allowed
-  if (!controls_allowed && (desired_torque != 0)) {
+  if (!is_lat_active() && (desired_torque != 0)) {
    violation = true;
  }

@@ -670,7 +678,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
  }

  // reset to 0 if either controls is not allowed or there's a violation
-  if (violation || !controls_allowed) {
+  if (violation || !is_lat_active()) {
    valid_steer_req_count = 0;
    invalid_steer_req_count = 0;
    desired_torque_last = 0;
@@ -686,7 +694,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const SteeringLi
 bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const SteeringLimits limits) {
  bool violation = false;

-  if (controls_allowed && steer_control_enabled) {
+  if (is_lat_active() && steer_control_enabled) {
    // convert floating point angle rate limits to integers in the scale of the desired angle on CAN,
    // add 1 to not false trigger the violation. also fudge the speed by 1 m/s so rate limits are
    // always slightly above openpilot's in case we read an updated speed in between angle commands
@@ -729,7 +737,7 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
  }

  // No angle control allowed when controls are not allowed
-  violation |= !controls_allowed && steer_control_enabled;
+  violation |= !is_lat_active() && steer_control_enabled;

  return violation;
 }
--- a/board/safety/safety_chrysler.h
+++ b/board/safety/safety_chrysler.h
@@ -78,6 +78,7 @@ static void chrysler_rx_hook(const CANPacket_t *to_push) {
  if ((bus == das_3_bus) && (addr == chrysler_addrs->DAS_3)) {
    bool cruise_engaged = GET_BIT(to_push, 21U);
    pcm_cruise_check(cruise_engaged);
+    acc_main_on = GET_BIT(to_push, 20U);
  }

  // TODO: use the same message for both
--- a/board/safety/safety_ford.h
+++ b/board/safety/safety_ford.h
@@ -180,6 +180,11 @@ static void ford_rx_hook(const CANPacket_t *to_push) {
      unsigned int cruise_state = GET_BYTE(to_push, 1) & 0x07U;
      bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U);
      pcm_cruise_check(cruise_engaged);
+      acc_main_on = (cruise_state == 3U) || cruise_engaged;
+    }
+    
+    if (addr == FORD_Steering_Data_FD1) {
+      lkas_button_press = GET_BIT(to_push, 40U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
    }

    // If steering controls messages are received on the destination bus, it's an indication
--- a/board/safety/safety_gm.h
+++ b/board/safety/safety_gm.h
@@ -85,6 +85,10 @@ static void gm_rx_hook(const CANPacket_t *to_push) {
      regen_braking = (GET_BYTE(to_push, 0) >> 4) != 0U;
    }

+    if (addr == 0xC9) {
+      acc_main_on = GET_BIT(to_push, 29U);
+    }
+
    bool stock_ecu_detected = (addr == 0x180);  // ASCMLKASteeringCmd

    // Check ASCMGasRegenCmd only if we're blocking it
--- a/board/safety/safety_honda.h
+++ b/board/safety/safety_honda.h
@@ -114,6 +114,13 @@ static void honda_rx_hook(const CANPacket_t *to_push) {
  // 0x1A6 for the ILX, 0x296 for the Civic Touring
  if (((addr == 0x1A6) || (addr == 0x296)) && (bus == pt_bus)) {
    int button = (GET_BYTE(to_push, 0) & 0xE0U) >> 5;
+    bool is_main = (button == HONDA_BTN_MAIN);
+    main_button_press = is_main ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
+    
+    int cruise_setting = (GET_BYTE(to_push, (addr == 0x296) ? 0U : 5U) & 0x0CU) >> 2U;
+    if (cruise_setting == 1) {
+      lkas_button_press = MADS_BUTTON_PRESSED;
+    }

    // enter controls on the falling edge of set or resume
    bool set = (button != HONDA_BTN_SET) && (cruise_button_prev == HONDA_BTN_SET);
--- a/board/safety/safety_hyundai.h
+++ b/board/safety/safety_hyundai.h
@@ -126,6 +126,12 @@ static void hyundai_rx_hook(const CANPacket_t *to_push) {
    hyundai_common_cruise_state_check(cruise_engaged);
  }

+  if ((addr == 0x420) && (((bus == 0) && !hyundai_camera_scc) || ((bus == 2) && hyundai_camera_scc))) {
+    if (!hyundai_longitudinal) {
+      acc_main_on = GET_BIT(to_push, 0U);
+    }
+  }
+
  if (bus == 0) {
    if (addr == 0x251) {
      int torque_driver_new = (GET_BYTES(to_push, 0, 2) & 0x7ffU) - 1024U;
@@ -133,6 +139,10 @@ static void hyundai_rx_hook(const CANPacket_t *to_push) {
      update_sample(&torque_driver, torque_driver_new);
    }

+    if (addr == 0x391) {
+      lkas_button_press = GET_BIT(to_push, 4U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
+    }
+
    // ACC steering wheel buttons
    if (addr == 0x4F1) {
      int cruise_button = GET_BYTE(to_push, 0) & 0x7U;
--- a/board/safety/safety_hyundai_canfd.h
+++ b/board/safety/safety_hyundai_canfd.h
@@ -68,9 +68,11 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *to_push) {
      if (addr == 0x1cf) {
        cruise_button = GET_BYTE(to_push, 2) & 0x7U;
        main_button = GET_BIT(to_push, 19U);
+        lkas_button_press = GET_BIT(to_push, 23U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
      } else {
        cruise_button = (GET_BYTE(to_push, 4) >> 4) & 0x7U;
        main_button = GET_BIT(to_push, 34U);
+        lkas_button_press = GET_BIT(to_push, 39U) ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
      }
      hyundai_common_cruise_buttons_check(cruise_button, main_button);
    }
--- a/board/safety/safety_hyundai_common.h
+++ b/board/safety/safety_hyundai_common.h
@@ -91,6 +91,8 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai
    hyundai_last_button_interaction = MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES);
  }

+  main_button_press = main_button ? MADS_BUTTON_PRESSED : MADS_BUTTON_NOT_PRESSED;
+
  if (hyundai_longitudinal) {
    // enter controls on falling edge of resume or set
    bool set = (cruise_button != HYUNDAI_BTN_SET) && (cruise_button_prev == HYUNDAI_BTN_SET);
--- a/board/safety/safety_mazda.h
+++ b/board/safety/safety_mazda.h
@@ -36,6 +36,7 @@ static void mazda_rx_hook(const CANPacket_t *to_push) {
    if (addr == MAZDA_CRZ_CTRL) {
      bool cruise_engaged = GET_BYTE(to_push, 0) & 0x8U;
      pcm_cruise_check(cruise_engaged);
+      acc_main_on = GET_BIT(to_push, 17U);
    }

    if (addr == MAZDA_ENGINE_DATA) {
--- a/board/safety/safety_nissan.h
+++ b/board/safety/safety_nissan.h
@@ -3,6 +3,7 @@
 #include "safety_declarations.h"

 static bool nissan_alt_eps = false;
+static bool nissan_leaf = false;

 static void nissan_rx_hook(const CANPacket_t *to_push) {
  int bus = GET_BUS(to_push);
@@ -53,6 +54,14 @@ static void nissan_rx_hook(const CANPacket_t *to_push) {
    pcm_cruise_check(cruise_engaged);
  }

+  if ((addr == 0x239) && (bus == 0) && nissan_leaf) {
+    acc_main_on = GET_BIT(to_push, 17U);
+  }
+
+  if ((addr == 0x1B6) && (bus == (nissan_alt_eps ? 2 : 1))) {
+    acc_main_on = GET_BIT(to_push, 36U);
+  }
+
  generic_rx_checks((addr == 0x169) && (bus == 0));
 }

@@ -150,8 +159,10 @@ static safety_config nissan_init(uint16_t param) {

  // EPS Location. false = V-CAN, true = C-CAN
  const int NISSAN_PARAM_ALT_EPS_BUS = 1;
+  const int NISSAN_PARAM_LEAF = 512;

  nissan_alt_eps = GET_FLAG(param, NISSAN_PARAM_ALT_EPS_BUS);
+  nissan_leaf = GET_FLAG(param, NISSAN_PARAM_LEAF);
  return BUILD_SAFETY_CFG(nissan_rx_checks, NISSAN_TX_MSGS);
 }

--- a/board/safety/safety_subaru.h
+++ b/board/safety/safety_subaru.h
@@ -106,10 +106,18 @@ static void subaru_rx_hook(const CANPacket_t *to_push) {
    update_sample(&angle_meas, angle_meas_new);
  }

+  if ((addr == MSG_SUBARU_ES_LKAS_State) && (bus == SUBARU_CAM_BUS)) {
+    int lkas_hud = (GET_BYTE(to_push, 2U) & 0x0CU) >> 2U;
+    if ((lkas_hud >= 1) && (lkas_hud <= 3)) {
+      lkas_button_press = MADS_BUTTON_PRESSED;
+    }
+  }
+
  // enter controls on rising edge of ACC, exit controls on ACC off
  if ((addr == MSG_SUBARU_CruiseControl) && (bus == alt_main_bus)) {
    bool cruise_engaged = GET_BIT(to_push, 41U);
    pcm_cruise_check(cruise_engaged);
+    acc_main_on = GET_BIT(to_push, 40U);
  }

  // update vehicle moving with any non-zero wheel speed
--- a/board/safety/safety_subaru_preglobal.h
+++ b/board/safety/safety_subaru_preglobal.h
@@ -36,6 +36,7 @@ static void subaru_preglobal_rx_hook(const CANPacket_t *to_push) {
    if (addr == MSG_SUBARU_PG_CruiseControl) {
      bool cruise_engaged = GET_BIT(to_push, 49U);
      pcm_cruise_check(cruise_engaged);
+      acc_main_on = GET_BIT(to_push, 48U);
    }

    // update vehicle moving with any non-zero wheel speed
--- a/board/safety/safety_toyota.h
+++ b/board/safety/safety_toyota.h
@@ -64,6 +64,17 @@ static bool toyota_get_quality_flag_valid(const CANPacket_t *to_push) {
 static void toyota_rx_hook(const CANPacket_t *to_push) {
  const int TOYOTA_LTA_MAX_ANGLE = 1657;  // EPS only accepts up to 94.9461

+  if (GET_BUS(to_push) == 2U) {
+    int addr = GET_ADDR(to_push);
+  
+    if (addr == 0x412) {
+      int lkas_hud = (GET_BYTE(to_push, 0U) & 0xC0U) >> 6U;
+      if ((lkas_hud >= 1) && (lkas_hud <= 3)) {
+        lkas_button_press = MADS_BUTTON_PRESSED;
+      }
+    }
+  }
+
  if (GET_BUS(to_push) == 0U) {
    int addr = GET_ADDR(to_push);

@@ -139,6 +150,10 @@ static void toyota_rx_hook(const CANPacket_t *to_push) {
      UPDATE_VEHICLE_SPEED(speed / 4.0 * 0.01 / 3.6);
    }

+    if (addr == 0x1D3) {
+      acc_main_on = GET_BIT(to_push, 15U);
+    }
+
    bool stock_ecu_detected = addr == 0x2E4;  // STEERING_LKA
    if (!toyota_stock_longitudinal && (addr == 0x343)) {
      stock_ecu_detected = true;  // ACC_CONTROL
--- a/board/sunnypilot/safety_mads.h
+++ b/board/sunnypilot/safety_mads.h
@@ -0,0 +1,246 @@
+#pragma once
+
+// Flags meant to be set by each specific safety_{make}
+typedef enum {
+  MADS_BUTTON_UNAVAILABLE = -1,
+  MADS_BUTTON_NOT_PRESSED = 0,
+  MADS_BUTTON_PRESSED = 1
+} ButtonState;
+
+extern ButtonState main_button_press;
+ButtonState main_button_press = MADS_BUTTON_UNAVAILABLE;
+
+extern ButtonState lkas_button_press;
+ButtonState lkas_button_press = MADS_BUTTON_UNAVAILABLE;
+
+// extern int temp_debug;
+// int temp_debug = 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.
+#define ALT_EXP_DISABLE_DISENGAGE_LATERAL_ON_BRAKE 2048
+
+#define MISMATCH_DEFAULT_THRESHOLD 25
+#define MADS_STATE_FLAG_DEFAULT 0U
+#define MADS_STATE_FLAG_RESERVED 1U
+#define MADS_STATE_FLAG_MAIN_BUTTON_AVAILABLE 2U
+#define MADS_STATE_FLAG_LKAS_BUTTON_AVAILABLE 4U
+
+
+// Button transition types
+typedef enum {
+  MADS_BUTTON_TRANSITION_NO_CHANGE,
+  MADS_BUTTON_TRANSITION_TO_PRESSED,
+  MADS_BUTTON_TRANSITION_TO_RELEASED
+} ButtonTransition;
+
+// MADS System State Struct
+typedef struct {
+  uint32_t state_flags;
+
+  // Values from stock that we need
+  const bool *is_vehicle_moving_ptr;
+
+  // System configuration flags
+  bool disengage_lateral_on_brake;
+
+  // System-wide enable/disable
+  bool system_enabled;
+
+  // Button states with last state tracking
+  struct {
+    const ButtonState *current;
+    ButtonState last;
+    ButtonTransition transition;
+    uint32_t press_timestamp;
+    bool is_engaged;
+  } main_button;
+
+  struct {
+    const ButtonState *current;
+    ButtonState last;
+    ButtonTransition transition;
+    uint32_t press_timestamp;
+    bool is_engaged;
+  } lkas_button; // Rule 12.3: separate declarations
+
+  // Vehicle condition states
+  bool is_braking;
+  bool cruise_engaged;
+
+  // Lateral control permission states
+  bool controls_allowed_lat;
+  bool disengaged_from_brakes;
+
+  // ACC main state tracking
+  struct {
+    const bool *current;
+    bool previous;
+    uint32_t mismatch_count;
+    uint32_t mismatch_threshold;
+  } acc_main;
+} MADSState;
+
+// Global state instance
+static MADSState _mads_state; // Rule 8.4: static for internal linkage
+
+// Determine button transition
+static ButtonTransition _get_button_transition(bool current, bool last) {
+  ButtonTransition result = MADS_BUTTON_TRANSITION_NO_CHANGE;
+
+  if (current && !last) {
+    result = MADS_BUTTON_TRANSITION_TO_PRESSED;
+  } else if (!current && last) {
+    result = MADS_BUTTON_TRANSITION_TO_RELEASED;
+  } else {
+    result = MADS_BUTTON_TRANSITION_NO_CHANGE;
+  }
+
+  return result;
+}
+
+// Initialize the MADS state
+static void mads_state_init(void) {
+  _mads_state.is_vehicle_moving_ptr = NULL;
+  _mads_state.acc_main.current = NULL;
+  _mads_state.main_button.current = &main_button_press;
+  _mads_state.lkas_button.current = &lkas_button_press;
+  _mads_state.state_flags = MADS_STATE_FLAG_DEFAULT;
+
+  _mads_state.system_enabled = false;
+  _mads_state.disengage_lateral_on_brake = true;
+
+  // Button state initialization
+  _mads_state.main_button.last = MADS_BUTTON_UNAVAILABLE;
+  _mads_state.main_button.transition = MADS_BUTTON_TRANSITION_NO_CHANGE;
+  _mads_state.main_button.press_timestamp = 0;
+  _mads_state.main_button.is_engaged = false;
+
+  _mads_state.lkas_button.last = MADS_BUTTON_UNAVAILABLE;
+  _mads_state.lkas_button.transition = MADS_BUTTON_TRANSITION_NO_CHANGE;
+  _mads_state.lkas_button.press_timestamp = 0;
+  _mads_state.lkas_button.is_engaged = false;
+
+  // ACC main state initialization
+  _mads_state.acc_main.previous = false;
+  _mads_state.acc_main.mismatch_count = 0;
+  _mads_state.acc_main.mismatch_threshold = MISMATCH_DEFAULT_THRESHOLD;
+
+  // Control states
+  _mads_state.is_braking = false;
+  _mads_state.cruise_engaged = false;
+  _mads_state.controls_allowed_lat = false;
+  _mads_state.disengaged_from_brakes = false;
+}
+
+// Exit lateral controls
+static void mads_exit_controls(void) {
+  if (_mads_state.controls_allowed_lat) {
+    _mads_state.disengaged_from_brakes = true;
+    _mads_state.controls_allowed_lat = false;
+  }
+}
+
+// Resume lateral controls
+static void _mads_resume_controls(void) {
+  if (_mads_state.disengaged_from_brakes) {
+    _mads_state.controls_allowed_lat = true;
+    _mads_state.disengaged_from_brakes = false;
+  }
+}
+
+// Check braking condition
+static void _mads_check_braking(bool is_braking) {
+  bool was_braking = _mads_state.is_braking;
+  if (is_braking && (!was_braking || *_mads_state.is_vehicle_moving_ptr) && _mads_state.disengage_lateral_on_brake) {
+    mads_exit_controls();
+  }
+
+  if (!is_braking && _mads_state.disengage_lateral_on_brake) {
+    _mads_resume_controls();
+  }
+  _mads_state.is_braking = is_braking;
+}
+
+// Update state based on input conditions
+void mads_state_update(const bool *op_vehicle_moving, const bool *op_acc_main, bool is_braking, bool cruise_engaged) {
+  if (_mads_state.is_vehicle_moving_ptr == NULL) {
+    _mads_state.is_vehicle_moving_ptr = op_vehicle_moving;
+  }
+
+  if (_mads_state.acc_main.current == NULL) {
+    _mads_state.acc_main.current = op_acc_main;
+  }
+
+  // Update button states
+  if (main_button_press != MADS_BUTTON_UNAVAILABLE) {
+    _mads_state.state_flags |= MADS_STATE_FLAG_MAIN_BUTTON_AVAILABLE;
+    _mads_state.main_button.current = &main_button_press;
+    _mads_state.main_button.transition = _get_button_transition(
+      *_mads_state.main_button.current == MADS_BUTTON_PRESSED,
+      _mads_state.main_button.last == MADS_BUTTON_PRESSED
+    );
+
+    // Engage on press, disengage on press if already pressed
+    if (_mads_state.main_button.transition == MADS_BUTTON_TRANSITION_TO_PRESSED) {
+      if (_mads_state.main_button.is_engaged) {
+        _mads_state.main_button.is_engaged = false; // Disengage if already engaged
+      } else {
+        _mads_state.main_button.is_engaged = true; // Engage otherwise
+      }
+    }
+
+    _mads_state.main_button.last = *_mads_state.main_button.current;
+  }
+
+  // Same for LKAS button
+  if (lkas_button_press != MADS_BUTTON_UNAVAILABLE) {
+    _mads_state.state_flags |= MADS_STATE_FLAG_LKAS_BUTTON_AVAILABLE;
+    _mads_state.lkas_button.current = &lkas_button_press;
+    _mads_state.lkas_button.transition = _get_button_transition(
+      *_mads_state.lkas_button.current == MADS_BUTTON_PRESSED,
+      _mads_state.lkas_button.last == MADS_BUTTON_PRESSED
+    );
+
+    if (_mads_state.lkas_button.transition == MADS_BUTTON_TRANSITION_TO_PRESSED) {
+      if (_mads_state.lkas_button.is_engaged) {
+        _mads_state.lkas_button.is_engaged = false;
+      } else {
+        _mads_state.lkas_button.is_engaged = true;
+      }
+    }
+
+    _mads_state.lkas_button.last = *_mads_state.lkas_button.current;
+  }
+
+  // Update other states
+  _mads_state.cruise_engaged = cruise_engaged;
+
+  //TODO-SP: theres a possibility of mismatching state if lat is engaged due to main button and disengaged due to lkas button. Need to validate if it's the case
+  // Use engagement state for lateral control
+  _mads_state.controls_allowed_lat = _mads_state.main_button.is_engaged || _mads_state.lkas_button.is_engaged || *
+                                     _mads_state.acc_main.current;
+
+  // Check ACC main state and braking conditions
+  _mads_check_braking(is_braking);
+
+  // Update ACC main state
+  _mads_state.acc_main.previous = *_mads_state.acc_main.current;
+}
+
+// Global system enable/disable
+void mads_set_system_state(bool enabled, bool disengage_lateral_on_brake) {
+  mads_state_init();
+  _mads_state.system_enabled = enabled;
+  _mads_state.disengage_lateral_on_brake = disengage_lateral_on_brake;
+}
+
+// Check if lateral control is currently allowed by MADS
+bool mads_is_lateral_control_allowed_by_mads(void) {
+  return _mads_state.system_enabled && _mads_state.controls_allowed_lat;
+}
--- a/python/init.py
+++ b/python/init.py
@@ -108,6 +108,10 @@ class ALTERNATIVE_EXPERIENCE:
  RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8
  ALLOW_AEB = 16

+  # sunnypilot
+  ENABLE_MADS = 2 ** 10
+  DISABLE_DISENGAGE_LATERAL_ON_BRAKE = 2 ** 11
+
 class Panda:

  # matches cereal.car.CarParams.SafetyModel
@@ -163,7 +167,7 @@ class Panda:
  CAN_PACKET_VERSION = 4
  HEALTH_PACKET_VERSION = 16
  CAN_HEALTH_PACKET_VERSION = 5
-  HEALTH_STRUCT = struct.Struct("<IIIIIIIIBBBBBHBBBHfBBHBHHB")
+  HEALTH_STRUCT = struct.Struct("<IIIIIIIIBBBBBHBBBHfBBHBHHBB")
  CAN_HEALTH_STRUCT = struct.Struct("<BIBBBBBBBBIIIIIIIHHBBBIIII")

  F4_DEVICES = [HW_TYPE_WHITE_PANDA, HW_TYPE_GREY_PANDA, HW_TYPE_BLACK_PANDA, HW_TYPE_UNO, HW_TYPE_DOS]
@@ -219,6 +223,7 @@ class Panda:
  FLAG_SUBARU_PREGLOBAL_REVERSED_DRIVER_TORQUE = 1

  FLAG_NISSAN_ALT_EPS_BUS = 1
+  FLAG_NISSAN_LEAF = 512

  FLAG_GM_HW_CAM = 1
  FLAG_GM_HW_CAM_LONG = 2
@@ -653,6 +658,7 @@ class Panda:
      "sbu1_voltage_mV": a[23],
      "sbu2_voltage_mV": a[24],
      "som_reset_triggered": a[25],
+      "controls_allowed_lat": a[26],
    }

  @ensure_can_health_packet_version
--- a/tests/libpanda/safety_helpers.h
+++ b/tests/libpanda/safety_helpers.h
@@ -35,6 +35,26 @@ bool get_controls_allowed(void){
  return controls_allowed;
 }

+bool get_lat_active(void){
+  return is_lat_active();
+}
+
+bool get_controls_allowed_lat(void){
+  return mads_is_lateral_control_allowed_by_mads();
+}
+
+void set_enable_mads(bool enable_mads, bool disengage_lat_on_brake){
+  mads_set_system_state(enable_mads, disengage_lat_on_brake);
+}
+
+bool get_enable_mads(void){
+  return _mads_state.system_enabled;
+}
+
+bool get_disengage_lat_on_brake(void){
+  return _mads_state.disengage_lateral_on_brake;
+}
+
 int get_alternative_experience(void){
  return alternative_experience;
 }
@@ -75,6 +95,10 @@ bool get_acc_main_on(void){
  return acc_main_on;
 }

+void set_acc_main_on(bool c){
+  acc_main_on = c;
+}
+
 int get_vehicle_speed_min(void){
  return vehicle_speed.min;
 }
@@ -181,6 +205,70 @@ bool get_honda_fwd_brake(void){
  return honda_fwd_brake;
 }

+int get_main_button_press(void){
+  return main_button_press;
+}
+
+void set_mads_state_flags(int flags){
+  _mads_state.state_flags = flags;
+}
+
+int get_mads_state_flags(void){
+  return _mads_state.state_flags; 
+}
+
+int get_mads_main_button_transition(void){
+  return _mads_state.main_button.transition;
+}
+
+int get_mads_main_button_current(void){
+  return *_mads_state.main_button.current;
+}
+
+int get_mads_main_button_last(void){
+  return _mads_state.main_button.last;
+}
+
+void set_main_button_press(int c){
+  main_button_press = c;
+}
+
+void set_lkas_button_press(int c){
+  lkas_button_press = c;
+}
+
+int get_lkas_button_press(void){
+  return lkas_button_press;
+}
+
+void set_controls_allowed_lat(bool c){
+  _mads_state.controls_allowed_lat = c;
+}
+
+bool get_main_button_engaged(void){
+  return _mads_state.main_button.is_engaged;
+}
+
+bool get_lkas_button_engaged(void){
+  return _mads_state.lkas_button.is_engaged;
+}
+
+void set_main_button_engaged(bool c){
+  _mads_state.main_button.is_engaged = c;
+}
+
+void set_lkas_button_engaged(bool c){
+  _mads_state.lkas_button.is_engaged = c;
+}
+
+int get_mads_acc_main(void){
+  return *_mads_state.acc_main.current;
+}
+
+//int get_temp_debug(void){
+//  return temp_debug;
+//}
+
 void init_tests(void){
  // get HW_TYPE from env variable set in test.sh
  if (getenv("HW_TYPE")) {
--- a/tests/libpanda/safety_helpers.py
+++ b/tests/libpanda/safety_helpers.py
@@ -5,6 +5,8 @@ def setup_safety_helpers(ffi):
  ffi.cdef("""
  void set_controls_allowed(bool c);
  bool get_controls_allowed(void);
+  bool get_lat_active(void);
+  bool get_controls_allowed_lat(void);
  bool get_longitudinal_allowed(void);
  void set_alternative_experience(int mode);
  int get_alternative_experience(void);
@@ -51,11 +53,44 @@ def setup_safety_helpers(ffi):
  void set_honda_alt_brake_msg(bool c);
  void set_honda_bosch_long(bool c);
  int get_honda_hw(void);
+
+  void set_enable_mads(bool enable_mads, bool disengage_lat_on_brake);
+  bool get_enable_mads(void);
+  bool get_disengage_lat_on_brake(void);
+  int get_main_button_press(void);
+  void set_mads_state_flags(int flags);
+  int get_mads_state_flags(void);
+  int get_mads_main_button_transition(void);
+  int get_mads_main_button_current(void);
+  int get_mads_main_button_last(void);
+  void set_main_button_press(int main_button_press);
+  void set_lkas_button_press(int lkas_button_press);
+  void set_controls_allowed_lat(bool c);
+
+  bool get_main_button_engaged(void);
+  bool get_lkas_button_engaged(void);
+  void set_main_button_engaged(bool engaged);
+  void set_lkas_button_engaged(bool engaged);
+
+  int get_mads_acc_main(void);
+  void set_acc_main_on(bool c);
+  int get_lkas_button_press(void);
+  // int get_temp_debug(void);
  """)

 class PandaSafety(Protocol):
  def set_controls_allowed(self, c: bool) -> None: ...
  def get_controls_allowed(self) -> bool: ...
+  def set_controls_allowed_lat(self, c: bool) -> None: ...
+  def get_lat_active(self) -> bool: ...
+  def get_controls_allowed_lat(self) -> bool: ...
+  def get_main_button_press(self) -> int: ...
+  def set_mads_state_flags(self, flags: int) -> None: ...
+  def get_mads_state_flags(self) -> int: ...
+  def get_mads_main_button_transition(self) -> int: ...
+  def get_mads_main_button_current(self) -> int: ...
+  def get_mads_main_button_last(self) -> int: ...
+  def get_mads_acc_main(self) -> int: ...
  def get_longitudinal_allowed(self) -> bool: ...
  def set_alternative_experience(self, mode: int) -> None: ...
  def get_alternative_experience(self) -> int: ...
@@ -66,6 +101,7 @@ class PandaSafety(Protocol):
  def get_brake_pressed_prev(self) -> bool: ...
  def get_regen_braking_prev(self) -> bool: ...
  def get_acc_main_on(self) -> bool: ...
+  def set_acc_main_on(self, c: bool) -> None: ...
  def get_vehicle_speed_min(self) -> int: ...
  def get_vehicle_speed_max(self) -> int: ...
  def get_vehicle_speed_last(self) -> int: ...
@@ -103,4 +139,15 @@ class PandaSafety(Protocol):
  def set_honda_bosch_long(self, c: bool) -> None: ...
  def get_honda_hw(self) -> int: ...

+  def set_enable_mads(self, enable_mads: bool, disengage_lat_on_brake: bool) -> None: ...
+  def set_main_button_press(self, main_button_press: int) -> None: ...
+  def set_lkas_button_press(self, lkas_button_press: int) -> None: ...
+  def get_enable_mads(self) -> bool: ...
+  def get_disengage_lat_on_brake(self) -> bool: ...
+  def set_main_button_engaged(self, engaged: bool) -> None: ...
+  def get_main_button_engaged(self) -> bool: ...
+  def set_lkas_button_engaged(self, engaged: bool) -> None: ...
+  def get_lkas_button_engaged(self) -> bool: ...

+  def get_lkas_button_press(self) -> int: ...
+  # def get_temp_debug(self) -> int: ...
--- a/tests/safety/common.py
+++ b/tests/safety/common.py
@@ -8,6 +8,7 @@ from collections.abc import Callable
 from opendbc.can.packer import CANPacker  # pylint: disable=import-error
 from panda import ALTERNATIVE_EXPERIENCE
 from panda.tests.libpanda import libpanda_py
+from panda.tests.safety.mads_common import MadsCommonBase

 MAX_WRONG_COUNTERS = 5
 MAX_SAMPLE_VALS = 6
@@ -813,7 +814,7 @@ class PandaSafetyTest(PandaSafetyTestBase):


@add_regen_tests
-class PandaCarSafetyTest(PandaSafetyTest):
+class PandaCarSafetyTest(PandaSafetyTest, MadsCommonBase):
  STANDSTILL_THRESHOLD: float | None = None
  GAS_PRESSED_THRESHOLD = 0
  RELAY_MALFUNCTION_ADDRS: dict[int, tuple[int, ...]] | None = None
@@ -985,6 +986,8 @@ class PandaCarSafetyTest(PandaSafetyTest):
  def test_safety_tick(self):
    self.safety.set_timer(int(2e6))
    self.safety.set_controls_allowed(True)
+    self.safety.set_controls_allowed_lat(True)
    self.safety.safety_tick_current_safety_config()
    self.assertFalse(self.safety.get_controls_allowed())
+    self.assertFalse(self.safety.get_controls_allowed_lat())
    self.assertFalse(self.safety.safety_config_valid())
--- a/tests/safety/mads_common.py
+++ b/tests/safety/mads_common.py
@@ -0,0 +1,238 @@
+import unittest
+from abc import abstractmethod
+from enum import IntFlag
+
+
+class MadsStates(IntFlag):
+  DEFAULT = 0
+  RESERVED = 1
+  MAIN_BUTTON_AVAILABLE = 2
+  LKAS_BUTTON_AVAILABLE = 4
+
+
+class MadsCommonBase(unittest.TestCase):
+  @abstractmethod
+  def _lkas_button_msg(self, enabled):
+    raise NotImplementedError
+
+  @abstractmethod
+  def _main_cruise_button_msg(self, enabled):
+    try:
+      self._button_msg(enabled)
+    except (NotImplementedError, AttributeError):
+      raise unittest.SkipTest("Skipping test because _button_msg is not implemented for this car. If you know it, please implement it.")
+
+    raise NotImplementedError("Since _button_msg is implemented, _main_cruise_button_msg should be implemented as well to signal the main cruise button press")
+
+  @abstractmethod
+  def _acc_state_msg(self, enabled):
+    raise NotImplementedError
+
+  def test_enable_control_from_cruise_button_press(self):
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        for cruise_button_press in [True, False]:
+          self.safety.set_enable_mads(enable_mads, False)
+          with self.subTest("cruise_button_press", cruise_button_press=cruise_button_press):
+            self._mads_states_cleanup()
+            self._rx(self._main_cruise_button_msg(cruise_button_press))
+            self.assertEqual(enable_mads and cruise_button_press, self.safety.get_controls_allowed_lat())
+    self._mads_states_cleanup()
+
+  def test_enable_control_from_lkas_button_press(self):
+    try:
+      self._lkas_button_msg(False)
+    except NotImplementedError:
+      raise unittest.SkipTest("Skipping test because _lkas_button_msg is not implemented for this car")
+
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        for lkas_button_press in [True, False]:
+          self.safety.set_enable_mads(enable_mads, False)
+          with self.subTest("lkas_button_press", button_state=lkas_button_press):
+            self._mads_states_cleanup()
+            self._rx(self._lkas_button_msg(lkas_button_press))
+            self.assertEqual(enable_mads and lkas_button_press, self.safety.get_controls_allowed_lat())
+    self._mads_states_cleanup()
+
+  def _mads_states_cleanup(self):
+    self.safety.set_main_button_press(-1)
+    self.safety.set_lkas_button_press(-1)
+    self.safety.set_controls_allowed_lat(False)
+    self.safety.set_main_button_engaged(False)
+    self.safety.set_lkas_button_engaged(False)
+    self.safety.set_mads_state_flags(0)
+    self.safety.set_acc_main_on(False)
+
+  def test_enable_control_from_setting_main_state_manually(self):
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        self.safety.set_enable_mads(enable_mads, False)
+        for main_button_press in (-1, 0, 1):
+          with self.subTest("main_button_press", button_state=main_button_press):
+            self._mads_states_cleanup()
+            self.safety.set_main_button_press(main_button_press)
+            self._rx(self._speed_msg(0))
+            self.assertEqual(enable_mads and main_button_press == 1, self.safety.get_controls_allowed_lat())
+    self._mads_states_cleanup()
+
+  def test_enable_control_from_setting_lkas_state_manually(self):
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        self.safety.set_enable_mads(enable_mads, False)
+        for lkas_button_press in (-1, 0, 1):
+          with self.subTest("lkas_button_press", button_state=lkas_button_press):
+            self._mads_states_cleanup()
+            self.safety.set_lkas_button_press(lkas_button_press)
+            self._rx(self._speed_msg(0))
+            self.assertEqual(enable_mads and lkas_button_press == 1, self.safety.get_controls_allowed_lat())
+    self._mads_states_cleanup()
+
+  def test_mads_state_flags(self):
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        self.safety.set_enable_mads(enable_mads, False)
+        self._mads_states_cleanup()
+        self.safety.set_main_button_press(0)  # Meaning a message with those buttons was seen and the _prev inside is no longer -1
+        self.safety.set_lkas_button_press(0)  # Meaning a message with those buttons was seen and the _prev inside is no longer -1
+        self._rx(self._speed_msg(0))
+        self.assertTrue(self.safety.get_mads_state_flags() & MadsStates.MAIN_BUTTON_AVAILABLE)
+        self.assertTrue(self.safety.get_mads_state_flags() & MadsStates.LKAS_BUTTON_AVAILABLE)
+    self._mads_states_cleanup()
+
+  def test_enable_control_from_acc_main_on(self):
+    """Test that lateral controls are allowed when ACC main is enabled"""
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        self.safety.set_enable_mads(enable_mads, False)
+        for acc_main_on in (True, False):
+          with self.subTest("acc_main_on", acc_main_on=acc_main_on):
+            self._mads_states_cleanup()
+            self.safety.set_acc_main_on(acc_main_on)
+            self._rx(self._speed_msg(0))
+            self.assertEqual(enable_mads and acc_main_on, self.safety.get_controls_allowed_lat())
+    self._mads_states_cleanup()
+
+  def test_controls_allowed_must_always_enable_lat(self):
+    for mads_enabled in [True, False]:
+      with self.subTest("mads enabled", mads_enabled=mads_enabled):
+        self.safety.set_enable_mads(mads_enabled, False)
+        for controls_allowed in [True, False]:
+          with self.subTest("controls allowed", controls_allowed=controls_allowed):
+            self.safety.set_controls_allowed(controls_allowed)
+            self.assertEqual(self.safety.get_controls_allowed(), self.safety.get_lat_active())
+
+  def test_mads_disengage_lat_on_brake_setup(self):
+    for mads_enabled in [True, False]:
+      with self.subTest("mads enabled", mads_enabled=mads_enabled):
+        for disengage_on_brake in [True, False]:
+          with self.subTest("disengage on brake", disengage_on_brake=disengage_on_brake):
+            self.safety.set_enable_mads(mads_enabled, disengage_on_brake)
+            self.assertEqual(disengage_on_brake, self.safety.get_disengage_lat_on_brake())
+
+  def test_mads_state_flags_mutation(self):
+    """Test to catch mutations in bitwise operations for state flags.
+    Specifically targets the mutation of & to | in flag checking operations.
+    Tests both setting and clearing of flags to catch potential bitwise operation mutations."""
+
+    # Test both MADS enabled and disabled states
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        self.safety.set_enable_mads(enable_mads, False)
+        self._mads_states_cleanup()
+
+        # Initial state - both flags should be unset
+        self._rx(self._speed_msg(0))
+        initial_flags = self.safety.get_mads_state_flags()
+        self.assertEqual(initial_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.DEFAULT)  # Main button flag
+        self.assertEqual(initial_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.DEFAULT)  # LKAS button flag
+
+        # Set only main button
+        self.safety.set_main_button_press(0)
+        self._rx(self._speed_msg(0))
+        main_only_flags = self.safety.get_mads_state_flags()
+        self.assertEqual(main_only_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.MAIN_BUTTON_AVAILABLE)  # Main button flag should be set
+        self.assertEqual(main_only_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.DEFAULT)  # LKAS button flag should still be unset
+
+        # Set LKAS button and verify both flags
+        self.safety.set_lkas_button_press(0)
+        self._rx(self._speed_msg(0))
+        both_flags = self.safety.get_mads_state_flags()
+        self.assertEqual(both_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.MAIN_BUTTON_AVAILABLE)  # Main button flag should remain set
+        self.assertEqual(both_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.LKAS_BUTTON_AVAILABLE)  # LKAS button flag should be set
+
+        # Verify that using | instead of & would give different results
+        self.assertNotEqual(both_flags & MadsStates.MAIN_BUTTON_AVAILABLE, both_flags | MadsStates.MAIN_BUTTON_AVAILABLE)
+        self.assertNotEqual(both_flags & MadsStates.LKAS_BUTTON_AVAILABLE, both_flags | MadsStates.LKAS_BUTTON_AVAILABLE)
+
+        # Reset flags and verify they're cleared
+        self._mads_states_cleanup()
+        self._rx(self._speed_msg(0))
+        cleared_flags = self.safety.get_mads_state_flags()
+        self.assertEqual(cleared_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.DEFAULT)
+        self.assertEqual(cleared_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.DEFAULT)
+
+  def test_mads_state_flags_persistence(self):
+    """Test to verify that state flags remain set once buttons are seen"""
+
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        self.safety.set_enable_mads(enable_mads, False)
+        self._mads_states_cleanup()
+
+        # Set main button and verify flag
+        self.safety.set_main_button_press(0)
+        self._rx(self._speed_msg(0))
+        self.assertEqual(self.safety.get_mads_state_flags() & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.MAIN_BUTTON_AVAILABLE)
+
+        # Reset main button to -1, flag should persist
+        self.safety.set_main_button_press(-1)
+        self._rx(self._speed_msg(0))
+        self.assertEqual(self.safety.get_mads_state_flags() & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.MAIN_BUTTON_AVAILABLE)
+
+        # Set LKAS button and verify both flags
+        self.safety.set_lkas_button_press(0)
+        self._rx(self._speed_msg(0))
+        flags = self.safety.get_mads_state_flags()
+        self.assertEqual(flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.MAIN_BUTTON_AVAILABLE)
+        self.assertEqual(flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.LKAS_BUTTON_AVAILABLE)
+
+  def test_mads_state_flags_individual_control(self):
+    """Test the ability to individually control state flags.
+    Verifies that flags can be set and cleared independently."""
+
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        self.safety.set_enable_mads(enable_mads, False)
+        self._mads_states_cleanup()
+
+        # Set main button flag only
+        self.safety.set_main_button_press(0)
+        self._rx(self._speed_msg(0))
+        main_flags = self.safety.get_mads_state_flags()
+        self.assertEqual(main_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.MAIN_BUTTON_AVAILABLE)
+        self.assertEqual(main_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.DEFAULT)
+
+        # Reset flags and set LKAS only
+        self._mads_states_cleanup()
+        self.safety.set_lkas_button_press(0)
+        self._rx(self._speed_msg(0))
+        lkas_flags = self.safety.get_mads_state_flags()
+        self.assertEqual(lkas_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.DEFAULT)
+        self.assertEqual(lkas_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.LKAS_BUTTON_AVAILABLE)
+
+        # Set both flags
+        self._mads_states_cleanup()
+        self.safety.set_main_button_press(0)
+        self.safety.set_lkas_button_press(0)
+        self._rx(self._speed_msg(0))
+        both_flags = self.safety.get_mads_state_flags()
+        self.assertEqual(both_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.MAIN_BUTTON_AVAILABLE)
+        self.assertEqual(both_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.LKAS_BUTTON_AVAILABLE)
+
+        # Clear all flags and verify
+        self._mads_states_cleanup()
+        self._rx(self._speed_msg(0))
+        final_flags = self.safety.get_mads_state_flags()
+        self.assertEqual(final_flags & MadsStates.MAIN_BUTTON_AVAILABLE, MadsStates.DEFAULT)
+        self.assertEqual(final_flags & MadsStates.LKAS_BUTTON_AVAILABLE, MadsStates.DEFAULT)
--- a/tests/safety/test_chrysler.py
+++ b/tests/safety/test_chrysler.py
@@ -72,6 +72,9 @@ class TestChryslerSafety(common.PandaCarSafetyTest, common.MotorTorqueSteeringSa
      self.assertFalse(self._tx(self._button_msg(cancel=True, resume=True)))
      self.assertFalse(self._tx(self._button_msg(cancel=False, resume=False)))

+  def test_enable_control_from_cruise_button_press(self):
+    raise unittest.SkipTest("Chrysler does not enable controls from cruise button press, we only enable controls from the PCM status message")
+

 class TestChryslerRamDTSafety(TestChryslerSafety):
  TX_MSGS = [[0xB1, 2], [0xA6, 0], [0xFA, 0]]
--- a/tests/safety/test_ford.py
+++ b/tests/safety/test_ford.py
@@ -168,6 +168,17 @@ class TestFordSafetyBase(common.PandaCarSafetyTest):
    }
    return self.packer.make_can_msg_panda("Lane_Assist_Data1", 0, values)

+  def test_enable_control_from_main(self):
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        self.safety.set_enable_mads(enable_mads, False)
+        for main_button_msg_valid in (True, False):
+          with self.subTest("main_button_msg_valid", state_valid=main_button_msg_valid):
+            self._mads_states_cleanup()
+            self._rx(self._pcm_status_msg(main_button_msg_valid))
+            self.assertEqual(enable_mads and main_button_msg_valid, self.safety.get_controls_allowed_lat())
+    self._mads_states_cleanup()
+
  # LCA command
  def _lat_ctl_msg(self, enabled: bool, path_offset: float, path_angle: float, curvature: float, curvature_rate: float):
    if self.STEER_MESSAGE == MSG_LateralMotionControl:
--- a/tests/safety/test_gm.py
+++ b/tests/safety/test_gm.py
@@ -140,6 +140,9 @@ class TestGmSafetyBase(common.PandaCarSafetyTest, common.DriverTorqueSteeringSaf
    values = {"ACCButtons": buttons}
    return self.packer.make_can_msg_panda("ASCMSteeringButton", self.BUTTONS_BUS, values)

+  def test_enable_control_from_cruise_button_press(self):
+    raise unittest.SkipTest("GM does not enable controls from cruise button press, we only enable controls from the PCM status message")
+

 class TestGmAscmSafety(GmLongitudinalBase, TestGmSafetyBase):
  TX_MSGS = [[0x180, 0], [0x409, 0], [0x40A, 0], [0x2CB, 0], [0x370, 0],  # pt bus
--- a/tests/safety/test_honda.py
+++ b/tests/safety/test_honda.py
@@ -250,6 +250,25 @@ class HondaBase(common.PandaCarSafetyTest):
    self.assertTrue(self._tx(self._send_steer_msg(0x0000)))
    self.assertFalse(self._tx(self._send_steer_msg(0x1000)))

+  def _lkas_button_msg(self, setting_btn):
+    values = {"CRUISE_SETTING": setting_btn, "COUNTER": self.cnt_button % 4}
+    self.__class__.cnt_button += 1
+    return self.packer.make_can_msg_panda("SCM_BUTTONS", self.PT_BUS, values)
+
+  def _main_cruise_button_msg(self, enabled):
+    return self._button_msg(Btn.MAIN if enabled else Btn.NONE, main_on=enabled)
+
+  def test_enable_control_from_lkas_button_press(self):
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        self.safety.set_enable_mads(enable_mads, False)
+        for lkas_button_press in range(3):
+          with self.subTest("lkas_button_press", button_state=lkas_button_press):
+            self._mads_states_cleanup()
+            self._rx(self._lkas_button_msg(lkas_button_press))
+            self.assertEqual(enable_mads and lkas_button_press == 1, self.safety.get_controls_allowed_lat())
+    self._mads_states_cleanup()
+

 # ********************* Honda Nidec **********************

@@ -371,6 +390,11 @@ class TestHondaNidecPcmAltSafety(TestHondaNidecPcmSafety):
    self.__class__.cnt_button += 1
    return self.packer.make_can_msg_panda("SCM_BUTTONS", bus, values)

+  def _lkas_button_msg(self, setting_btn):
+    values = {"CRUISE_SETTING": setting_btn, "COUNTER": self.cnt_button % 4}
+    self.__class__.cnt_button += 1
+    return self.packer.make_can_msg_panda("SCM_BUTTONS", self.PT_BUS, values)
+

 # ********************* Honda Bosch **********************

--- a/tests/safety/test_hyundai.py
+++ b/tests/safety/test_hyundai.py
@@ -112,6 +112,33 @@ class TestHyundaiSafety(HyundaiButtonBase, common.PandaCarSafetyTest, common.Dri
    values = {"CR_Lkas_StrToqReq": torque, "CF_Lkas_ActToi": steer_req}
    return self.packer.make_can_msg_panda("LKAS11", 0, values)

+  def _acc_state_msg(self, enable):
+    values = {"MainMode_ACC": enable, "AliveCounterACC": self.cnt_cruise % 16}
+    self.__class__.cnt_cruise += 1
+    return self.packer.make_can_msg_panda("SCC11", self.SCC_BUS, values)
+
+  def _lkas_button_msg(self, enabled):
+    values = {"LFA_Pressed": enabled}
+    return self.packer.make_can_msg_panda("BCM_PO_11", 0, values)
+
+  def _main_cruise_button_msg(self, enabled):
+    return self._button_msg(0, enabled)
+
+  def test_acc_main_state_from_stock_scc_message(self):
+    """Test that ACC main state is correctly set when receiving 0x420 message, toggling HYUNDAI_LONG flag"""
+    prior_safety_mode = self.safety.get_current_safety_mode()
+    prior_safety_param = self.safety.get_current_safety_param()
+
+    for hyundai_longitudinal in (True, False):
+      with self.subTest("hyundai_longitudinal", hyundai_longitudinal=hyundai_longitudinal):
+        self.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI, 0 if hyundai_longitudinal else Panda.FLAG_HYUNDAI_LONG)
+        for should_turn_acc_main_on in (True, False):
+          with self.subTest("acc_main_on", should_turn_acc_main_on=should_turn_acc_main_on):
+            self._rx(self._acc_state_msg(should_turn_acc_main_on))  # Send the ACC state message
+            expected_acc_main = should_turn_acc_main_on and hyundai_longitudinal  # ACC main should only be set if hyundai_longitudinal is True
+            self.assertEqual(expected_acc_main, self.safety.get_acc_main_on())
+    self.safety.set_safety_hooks(prior_safety_mode, prior_safety_param)
+

 class TestHyundaiSafetyAltLimits(TestHyundaiSafety):
  MAX_RATE_UP = 2
@@ -135,6 +162,21 @@ class TestHyundaiSafetyCameraSCC(TestHyundaiSafety):
    self.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI, Panda.FLAG_HYUNDAI_CAMERA_SCC)
    self.safety.init_tests()

+  def test_acc_main_state_from_stock_scc_message(self):
+    """
+    Test that ACC main state is correctly set when receiving 0x420 message.
+    For camera SCC, ACC main should always be on when receiving 0x420 message
+    """
+    prior_safety_mode = self.safety.get_current_safety_mode()
+    prior_safety_param = self.safety.get_current_safety_param()
+
+    for should_turn_acc_main_on in (True, False):
+      with self.subTest("acc_main_on", should_turn_acc_main_on=should_turn_acc_main_on):
+        self._rx(self._acc_state_msg(should_turn_acc_main_on))
+        self.assertEqual(should_turn_acc_main_on, self.safety.get_acc_main_on())
+    # Restore original safety mode and param
+    self.safety.set_safety_hooks(prior_safety_mode, prior_safety_param)
+

 class TestHyundaiLegacySafety(TestHyundaiSafety):
  def setUp(self):
--- a/tests/safety/test_hyundai_canfd.py
+++ b/tests/safety/test_hyundai_canfd.py
@@ -78,6 +78,28 @@ class TestHyundaiCanfdBase(HyundaiButtonBase, common.PandaCarSafetyTest, common.
    }
    return self.packer.make_can_msg_panda("CRUISE_BUTTONS", bus, values)

+  def _acc_state_msg(self, enable):
+    values = {"MainMode_ACC": enable}
+    return self.packer.make_can_msg_panda("SCC_CONTROL", self.SCC_BUS, values)
+
+  def _lkas_button_msg(self, enabled):
+    values = {"LKAS_BTN": enabled}
+    return self.packer.make_can_msg_panda("CRUISE_BUTTONS", self.PT_BUS, values)
+
+  def _main_cruise_button_msg(self, enabled):
+    return self._button_msg(0, enabled)
+
+  def test_enable_control_from_lkas(self):
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        self.safety.set_enable_mads(enable_mads, False)
+        for lkas_button_msg_valid in (True, False):
+          with self.subTest("main_button_msg_valid", state_valid=lkas_button_msg_valid):
+            self._mads_states_cleanup()
+            self._rx(self._lkas_button_msg(lkas_button_msg_valid))
+            self.assertEqual(enable_mads and lkas_button_msg_valid, self.safety.get_controls_allowed_lat())
+    self._mads_states_cleanup()
+

 class TestHyundaiCanfdHDA1Base(TestHyundaiCanfdBase):

@@ -146,6 +168,10 @@ class TestHyundaiCanfdHDA1AltButtons(TestHyundaiCanfdHDA1Base):
    }
    return self.packer.make_can_msg_panda("CRUISE_BUTTONS_ALT", self.PT_BUS, values)

+  def _lkas_button_msg(self, enabled):
+    values = {"LFA_BTN": enabled}
+    return self.packer.make_can_msg_panda("CRUISE_BUTTONS_ALT", self.PT_BUS, values)
+
  def test_button_sends(self):
    """
      No button send allowed with alt buttons.
--- a/tests/safety/test_mazda.py
+++ b/tests/safety/test_mazda.py
@@ -81,6 +81,9 @@ class TestMazdaSafety(common.PandaCarSafetyTest, common.DriverTorqueSteeringSafe
    self.assertTrue(self._tx(self._button_msg(cancel=True)))
    self.assertTrue(self._tx(self._button_msg(resume=True)))

+  def test_enable_control_from_cruise_button_press(self):
+    raise unittest.SkipTest("Mazda does not enable controls from cruise button press, we only enable controls from the PCM status message")
+

 if __name__ == "__main__":
  unittest.main()
--- a/tests/safety/test_nissan.py
+++ b/tests/safety/test_nissan.py
@@ -17,6 +17,7 @@ class TestNissanSafety(common.PandaCarSafetyTest, common.AngleSteeringSafetyTest

  EPS_BUS = 0
  CRUISE_BUS = 2
+  ACC_MAIN_BUS = 1

  # Angle control limits
  DEG_TO_CAN = 100
@@ -55,6 +56,10 @@ class TestNissanSafety(common.PandaCarSafetyTest, common.AngleSteeringSafetyTest
    values = {"GAS_PEDAL": gas}
    return self.packer.make_can_msg_panda("GAS_PEDAL", self.EPS_BUS, values)

+  def _acc_state_msg(self, main_on):
+    values = {"CRUISE_ON": main_on}
+    return self.packer.make_can_msg_panda("PRO_PILOT", self.ACC_MAIN_BUS, values)
+
  def _acc_button_cmd(self, cancel=0, propilot=0, flw_dist=0, _set=0, res=0):
    no_button = not any([cancel, propilot, flw_dist, _set, res])
    values = {"CANCEL_BUTTON": cancel, "PROPILOT_BUTTON": propilot,
@@ -78,12 +83,26 @@ class TestNissanSafety(common.PandaCarSafetyTest, common.AngleSteeringSafetyTest
        tx = self._tx(self._acc_button_cmd(**args))
        self.assertEqual(tx, should_tx)

+  def test_enable_control_from_acc_main_on(self):
+    """Test that lateral controls are allowed when ACC main is enabled"""
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        self.safety.set_enable_mads(enable_mads, False)
+        for acc_main_on in (True, False):
+          with self.subTest("acc_main_on", acc_main_on=acc_main_on):
+            self._mads_states_cleanup()
+            self._rx(self._acc_state_msg(acc_main_on))
+            self._rx(self._speed_msg(0))
+            self.assertEqual(enable_mads and acc_main_on, self.safety.get_controls_allowed_lat())
+    self._mads_states_cleanup()
+

 class TestNissanSafetyAltEpsBus(TestNissanSafety):
  """Altima uses different buses"""

  EPS_BUS = 1
  CRUISE_BUS = 1
+  ACC_MAIN_BUS = 2

  def setUp(self):
    self.packer = CANPackerPanda("nissan_x_trail_2017_generated")
@@ -91,13 +110,17 @@ class TestNissanSafetyAltEpsBus(TestNissanSafety):
    self.safety.set_safety_hooks(Panda.SAFETY_NISSAN, Panda.FLAG_NISSAN_ALT_EPS_BUS)
    self.safety.init_tests()

+  def _acc_state_msg(self, main_on):
+    values = {"CRUISE_ON": main_on}
+    return self.packer.make_can_msg_panda("PRO_PILOT", self.ACC_MAIN_BUS, values)
+

 class TestNissanLeafSafety(TestNissanSafety):

  def setUp(self):
    self.packer = CANPackerPanda("nissan_leaf_2018_generated")
    self.safety = libpanda_py.libpanda
-    self.safety.set_safety_hooks(Panda.SAFETY_NISSAN, 0)
+    self.safety.set_safety_hooks(Panda.SAFETY_NISSAN, Panda.FLAG_NISSAN_LEAF)
    self.safety.init_tests()

  def _user_brake_msg(self, brake):
@@ -108,6 +131,10 @@ class TestNissanLeafSafety(TestNissanSafety):
    values = {"GAS_PEDAL": gas}
    return self.packer.make_can_msg_panda("CRUISE_THROTTLE", 0, values)

+  def _acc_state_msg(self, main_on):
+    values = {"CRUISE_AVAILABLE": main_on}
+    return self.packer.make_can_msg_panda("CRUISE_THROTTLE", 0, values)
+
  # TODO: leaf should use its own safety param
  def test_acc_buttons(self):
    pass
--- a/tests/safety/test_subaru.py
+++ b/tests/safety/test_subaru.py
@@ -106,6 +106,21 @@ class TestSubaruSafetyBase(common.PandaCarSafetyTest):
    values = {"Cruise_Activated": enable}
    return self.packer.make_can_msg_panda("CruiseControl", self.ALT_MAIN_BUS, values)

+  def _lkas_button_msg(self, lkas_hud):
+    values = {"LKAS_Dash_State": lkas_hud}
+    return self.packer.make_can_msg_panda("ES_LKAS_State", SUBARU_CAM_BUS, values)
+
+  def test_enable_control_from_lkas_button_press(self):
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        for lkas_hud in range(4):
+          self.safety.set_enable_mads(enable_mads, False)
+          self._mads_states_cleanup()
+          with self.subTest("lkas_hud", button_state=lkas_hud):
+            self._rx(self._lkas_button_msg(lkas_hud))
+            self.assertEqual(enable_mads and lkas_hud in range(1, 4), self.safety.get_controls_allowed_lat())
+    self._mads_states_cleanup()
+

 class TestSubaruStockLongitudinalSafetyBase(TestSubaruSafetyBase):
  def _cancel_msg(self, cancel, cruise_throttle=0):
--- a/tests/safety/test_toyota.py
+++ b/tests/safety/test_toyota.py
@@ -81,6 +81,10 @@ class TestToyotaSafetyBase(common.PandaCarSafetyTest, common.LongitudinalAccelSa
    values = {"CRUISE_ACTIVE": enable}
    return self.packer.make_can_msg_panda("PCM_CRUISE", 0, values)

+  def _lkas_button_msg(self, lkas_hud):
+    values = {"LKAS_STATUS": lkas_hud}
+    return self.packer.make_can_msg_panda("LKAS_HUD", 2, values)
+
  def test_diagnostics(self, stock_longitudinal: bool = False):
    for should_tx, msg in ((False, b"\x6D\x02\x3E\x00\x00\x00\x00\x00"),  # fwdCamera tester present
                           (False, b"\x0F\x03\xAA\xAA\x00\x00\x00\x00"),  # non-tester present
@@ -127,6 +131,18 @@ class TestToyotaSafetyBase(common.PandaCarSafetyTest, common.LongitudinalAccelSa
      self.assertFalse(self._rx(to_push))
      self.assertFalse(self.safety.get_controls_allowed())

+  def test_enable_control_from_lkas_button_press(self):
+    for enable_mads in (True, False):
+      with self.subTest("enable_mads", mads_enabled=enable_mads):
+        for lkas_hud in range(4):
+          self.safety.set_enable_mads(enable_mads, False)
+          self._mads_states_cleanup()
+          with self.subTest("lkas_hud", button_state=lkas_hud):
+            self._rx(self._lkas_button_msg(lkas_hud))
+            self._rx(self._speed_msg(0))  # Only for Toyota, we must send a msg to bus 0 because generic_rx_checks happen only there.
+            self.assertEqual(enable_mads and lkas_hud in range(1, 4), self.safety.get_controls_allowed_lat())
+    self._mads_states_cleanup()
+

 class TestToyotaSafetyTorque(TestToyotaSafetyBase, common.MotorTorqueSteeringSafetyTest, common.SteerRequestCutSafetyTest):

--- a/tests/safety/test_volkswagen_mqb.py
+++ b/tests/safety/test_volkswagen_mqb.py
@@ -134,6 +134,8 @@ class TestVolkswagenMqbSafety(common.PandaCarSafetyTest, common.DriverTorqueStee
    self.assertEqual(0, self.safety.get_torque_driver_max())
    self.assertEqual(0, self.safety.get_torque_driver_min())

+  def test_enable_control_from_cruise_button_press(self):
+    raise unittest.SkipTest("Volkswagen does not enable controls from cruise button press, we only enable controls from the PCM status message")

 class TestVolkswagenMqbStockSafety(TestVolkswagenMqbSafety):
  TX_MSGS = [[MSG_HCA_01, 0], [MSG_LDW_02, 0], [MSG_LH_EPS_03, 2], [MSG_GRA_ACC_01, 0], [MSG_GRA_ACC_01, 2]]
--- a/tests/safety/test_volkswagen_pq.py
+++ b/tests/safety/test_volkswagen_pq.py
@@ -115,6 +115,9 @@ class TestVolkswagenPqSafety(common.PandaCarSafetyTest, common.DriverTorqueSteer
    self.assertEqual(0, self.safety.get_torque_driver_max())
    self.assertEqual(0, self.safety.get_torque_driver_min())

+  def test_enable_control_from_cruise_button_press(self):
+    raise unittest.SkipTest("Volkswagen does not enable controls from cruise button press, we only enable controls from the PCM status message")
+

 class TestVolkswagenPqStockSafety(TestVolkswagenPqSafety):
  # Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
--- a/tests/safety_replay/replay_drive.py
+++ b/tests/safety_replay/replay_drive.py
@@ -13,6 +13,7 @@ def replay_drive(lr, safety_mode, param, alternative_experience, segment=False):
  err = safety.set_safety_hooks(safety_mode, param)
  assert err == 0, "invalid safety mode: %d" % safety_mode
  safety.set_alternative_experience(alternative_experience)
+  safety.set_enable_mads(bool(alternative_experience & 1024), not bool(alternative_experience & 2048))

  if segment:
    init_segment(safety, lr, safety_mode, param)
@@ -44,7 +45,11 @@ def replay_drive(lr, safety_mode, param, alternative_experience, segment=False):
          blocked_addrs[canmsg.address] += 1

          if "DEBUG" in os.environ:
-            print("blocked bus %d msg %d at %f" % (canmsg.src, canmsg.address, (msg.logMonoTime - start_t) / 1e9))
+            print(f"blocked bus {canmsg.src} msg {hex(canmsg.address)} at {(msg.logMonoTime - start_t) / 1e9} | " +
+                  f"lat [{safety.get_lat_active()}] | alwd [{safety.get_controls_allowed_lat()}] | " +
+                  f"main_on [{safety.get_acc_main_on()}] | " +
+                  f"flags [{safety.get_mads_state_flags()}] | " +
+                  f"mads_main [{safety.get_mads_acc_main()}] ")
        tx_controls += safety.get_controls_allowed()
        tx_tot += 1
    elif msg.which() == 'can':
@@ -68,6 +73,7 @@ def replay_drive(lr, safety_mode, param, alternative_experience, segment=False):
  print("blocked msgs:", tx_blocked)
  print("blocked with controls allowed:", tx_controls_blocked)
  print("blocked addrs:", blocked_addrs)
+  print("mads enabled:", safety.get_enable_mads())

  return tx_controls_blocked == 0 and rx_invalid == 0 and not safety_tick_rx_invalid