mawei/opendbc-meb
1
0
Fork 0
mirror of https://github.com/infiniteCable2/opendbc.git synced 2026-02-18 13:03:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert "MADS: Steering Mode on Brake Pedal Press (#92)" (#114)

Browse Source 
This reverts commit a26ea1e3ee.
This commit is contained in:
Jason Wen
2025-04-04 22:27:54 -04:00
committed by GitHub
parent 71d422f8fe
commit 2faae4b627
12 changed files with 62 additions and 81 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
opendbc/safety/__init__.py
View File

@@ -11,6 +11,5 @@ class ALTERNATIVE_EXPERIENCE:
ALLOW_AEB = 16
# sunnypilot
ENABLE_MADS = 1024
MADS_DISENGAGE_LATERAL_ON_BRAKE = 2048
MADS_PAUSE_LATERAL_ON_BRAKE = 4096
ENABLE_MADS = 2 ** 10
DISENGAGE_LATERAL_ON_BRAKE = 2 ** 11

14
opendbc/safety/sunnypilot/safety_mads.h
View File

@@ -45,7 +45,6 @@ inline void m_mads_state_init(void) {
m_mads_state.system_enabled = false;
m_mads_state.disengage_lateral_on_brake = false;
m_mads_state.pause_lateral_on_brake = false;
m_mads_state.acc_main.previous = false;
m_mads_state.acc_main.transition = MADS_EDGE_NO_CHANGE;
@@ -97,7 +96,7 @@ inline void m_update_control_state(void) {
}
// Secondary control conditions - only checked if primary conditions don't block further control processing
if (allowed && (m_mads_state.disengage_lateral_on_brake || m_mads_state.pause_lateral_on_brake)) {
if (allowed && m_mads_state.disengage_lateral_on_brake) {
// Brake rising edge immediately blocks controls
// Brake release might request controls if brake was the ONLY reason for disengagement
if (m_mads_state.braking.transition == MADS_EDGE_RISING) {
@@ -105,8 +104,7 @@ inline void m_update_control_state(void) {
allowed = false;
} else if ((m_mads_state.braking.transition == MADS_EDGE_FALLING) &&
(m_mads_state.current_disengage.active_reason == MADS_DISENGAGE_REASON_BRAKE) &&
(m_mads_state.current_disengage.pending_reasons == MADS_DISENGAGE_REASON_BRAKE) &&
m_mads_state.pause_lateral_on_brake) {
(m_mads_state.current_disengage.pending_reasons == MADS_DISENGAGE_REASON_BRAKE)) {
m_mads_state.controls_requested_lat = true;
} else if (m_mads_state.braking.current) {
allowed = false;
@@ -140,17 +138,15 @@ inline void mads_heartbeat_engaged_check(void) {
inline void mads_set_alternative_experience(const int *mode) {
const bool mads_enabled = (*mode & ALT_EXP_ENABLE_MADS) != 0;
const bool disengage_lateral_on_brake = (*mode & ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE) != 0;
const bool pause_lateral_on_brake = (*mode & ALT_EXP_MADS_PAUSE_LATERAL_ON_BRAKE) != 0;
const bool disengage_lateral_on_brake = (*mode & ALT_EXP_DISENGAGE_LATERAL_ON_BRAKE) != 0;
mads_set_system_state(mads_enabled, disengage_lateral_on_brake, pause_lateral_on_brake);
mads_set_system_state(mads_enabled, disengage_lateral_on_brake);
}
extern inline void mads_set_system_state(const bool enabled, const bool disengage_lateral_on_brake, const bool pause_lateral_on_brake) {
extern inline void mads_set_system_state(const bool enabled, const bool disengage_lateral_on_brake) {
m_mads_state_init();
m_mads_state.system_enabled = enabled;
m_mads_state.disengage_lateral_on_brake = disengage_lateral_on_brake;
m_mads_state.pause_lateral_on_brake = pause_lateral_on_brake;
}
inline void mads_exit_controls(const DisengageReason reason) {

8
opendbc/safety/sunnypilot/safety_mads_declarations.h
View File

@@ -38,8 +38,7 @@ typedef enum __attribute__((packed)) {
// ===============================
#define ALT_EXP_ENABLE_MADS 1024
#define ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE 2048
#define ALT_EXP_MADS_PAUSE_LATERAL_ON_BRAKE 4096
#define ALT_EXP_DISENGAGE_LATERAL_ON_BRAKE 2048
#define MISMATCH_DEFAULT_THRESHOLD 25
@@ -76,7 +75,6 @@ typedef struct {
bool system_enabled : 1;
bool disengage_lateral_on_brake : 1;
bool pause_lateral_on_brake : 1;
bool controls_requested_lat : 1;
bool controls_allowed_lat : 1;
} MADSState;
@@ -96,7 +94,7 @@ extern uint32_t heartbeat_engaged_mads_mismatches;
// External Function Declarations (kept as needed)
// ===============================
extern void mads_set_system_state(bool enabled, bool disengage_lateral_on_brake, bool pause_lateral_on_brake);
extern void mads_set_system_state(bool enabled, bool disengage_lateral_on_brake);
extern void mads_set_alternative_experience(const int *mode);
extern void mads_state_update(bool op_vehicle_moving, bool op_acc_main, bool op_allowed, bool is_braking);
extern void mads_exit_controls(DisengageReason reason);
@@ -111,4 +109,4 @@ extern EdgeTransition m_get_edge_transition(bool current, bool last);
extern void m_mads_state_init(void);
extern void m_update_button_state(ButtonStateTracking *button_state);
extern void m_update_binary_state(BinaryStateTracking *state);
extern void m_update_control_state(void);
extern void m_update_control_state(void);

8
opendbc/safety/tests/hyundai_common.py
View File

@@ -158,7 +158,7 @@ class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest):
# Test initial state
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
self.assertFalse(self.safety.get_acc_main_on())
@@ -191,7 +191,7 @@ class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest):
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
# Initial state
self._rx(self._main_cruise_button_msg(0))
@@ -226,7 +226,7 @@ class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest):
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
self.safety.set_controls_allowed_lat(True)
# Start with matched states
@@ -269,7 +269,7 @@ class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest):
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
# Create initial mismatch
self._rx(self._main_cruise_button_msg(1)) # Press button

14
opendbc/safety/tests/libsafety/safety_helpers.h
View File

@@ -60,10 +60,6 @@ bool get_disengage_lateral_on_brake(void){
return get_mads_state()->disengage_lateral_on_brake;
}
bool get_pause_lateral_on_brake(void){
return get_mads_state()->pause_lateral_on_brake;
}
int get_alternative_experience(void){
return alternative_experience;
}
@@ -242,17 +238,13 @@ void set_controls_requested_lat(bool c){
m_mads_state.controls_requested_lat = c;
}
void set_mads_params(bool enable_mads, bool disengage_lateral_on_brake, bool pause_lateral_on_brake){
void set_mads_params(bool enable_mads, bool disengage_lateral_on_brake){
alternative_experience = 0;
if (enable_mads) {
alternative_experience |= ALT_EXP_ENABLE_MADS;
if (disengage_lateral_on_brake) {
alternative_experience |= ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE;
} else if (pause_lateral_on_brake) {
alternative_experience |= ALT_EXP_MADS_PAUSE_LATERAL_ON_BRAKE;
} else {
}
if (disengage_lateral_on_brake)
alternative_experience |= ALT_EXP_DISENGAGE_LATERAL_ON_BRAKE;
}
mads_set_alternative_experience(&alternative_experience);

6
opendbc/safety/tests/libsafety/safety_helpers.py
View File

@@ -57,7 +57,6 @@ def setup_safety_helpers(ffi):
bool get_enable_mads(void);
bool get_disengage_lateral_on_brake(void);
bool get_pause_lateral_on_brake(void);
void set_mads_button_press(int mads_button_press);
void set_controls_allowed_lat(bool c);
void set_controls_requested_lat(bool c);
@@ -69,7 +68,7 @@ def setup_safety_helpers(ffi):
int mads_get_current_disengage_reason(void);
int get_temp_debug(void);
uint32_t get_acc_main_on_mismatches(void);
void set_mads_params(bool enable_mads, bool disengage_lateral_on_brake, bool pause_lateral_on_brake);
void set_mads_params(bool enable_mads, bool disengage_lat_on_brake);
void set_heartbeat_engaged_mads(bool c);
void mads_heartbeat_engaged_check(void);
""")
@@ -134,14 +133,13 @@ class PandaSafety(Protocol):
def set_mads_button_press(self, mads_button_press: int) -> None: ...
def get_enable_mads(self) -> bool: ...
def get_disengage_lateral_on_brake(self) -> bool: ...
def get_pause_lateral_on_brake(self) -> bool: ...
def get_mads_button_press(self) -> int: ...
def mads_set_current_disengage_reason(self, reason: int) -> None: ...
def mads_get_current_disengage_reason(self) -> int: ...
def get_acc_main_on_mismatches(self) -> int: ...
def set_mads_params(self, enable_mads: bool, disengage_lateral_on_brake: bool, pause_lateral_on_brake: bool) -> None: ...
def set_mads_params(self, enable_mads: bool, disengage_lat_on_brake: bool) -> None: ...
def set_heartbeat_engaged_mads(self, c: bool) -> None: ...
def mads_heartbeat_engaged_check(self) -> None: ...
# def get_temp_debug(self) -> int: ...

74
opendbc/safety/tests/mads_common.py
View File

@@ -20,7 +20,7 @@ class MadsSafetyTestBase(unittest.TestCase):
self.safety.set_controls_allowed_lat(False)
self.safety.set_controls_requested_lat(False)
self.safety.set_acc_main_on(False)
self.safety.set_mads_params(False, False, False)
self.safety.set_mads_params(False, False)
self.safety.set_heartbeat_engaged_mads(True)
def test_heartbeat_engaged_mads_check(self):
@@ -30,7 +30,7 @@ class MadsSafetyTestBase(unittest.TestCase):
with self.subTest(heartbeat_engaged=boolean, should_remain_engaged=boolean):
# Setup initial conditions
self._mads_states_cleanup()
self.safety.set_mads_params(True, False, False) # Enable MADS
self.safety.set_mads_params(True, False) # Enable MADS
self.safety.set_controls_allowed_lat(True)
self.assertTrue(self.safety.get_controls_allowed_lat())
@@ -57,7 +57,7 @@ class MadsSafetyTestBase(unittest.TestCase):
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
self.assertEqual(enable_mads, self.safety.get_enable_mads())
self._rx(self._lkas_button_msg(True))
@@ -79,7 +79,7 @@ class MadsSafetyTestBase(unittest.TestCase):
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
self._rx(self._acc_state_msg(True))
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat())
@@ -93,7 +93,7 @@ class MadsSafetyTestBase(unittest.TestCase):
for mads_button_press in (-1, 0, 1):
with self.subTest("mads_button_press", button_state=mads_button_press):
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
self.safety.set_mads_button_press(mads_button_press)
self._rx(self._speed_msg(0))
@@ -109,7 +109,7 @@ class MadsSafetyTestBase(unittest.TestCase):
for acc_main_on in (True, False):
with self.subTest("initial_acc_main", initial_acc_main=acc_main_on):
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
# Set initial state
self.safety.set_acc_main_on(acc_main_on)
@@ -139,7 +139,7 @@ class MadsSafetyTestBase(unittest.TestCase):
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
self.safety.set_acc_main_on(True)
self._rx(self._speed_msg(0))
@@ -151,22 +151,22 @@ class MadsSafetyTestBase(unittest.TestCase):
finally:
self._mads_states_cleanup()
def test_pause_lateral_on_brake_setup(self):
def test_disengage_lateral_on_brake_setup(self):
try:
for enable_mads in (True, False):
with self.subTest("enable_mads", enable_mads=enable_mads):
for pause_lateral_on_brake in (True, False):
with self.subTest("pause_lateral_on_brake", pause_lateral_on_brake=pause_lateral_on_brake):
for disengage_on_brake in (True, False):
with self.subTest("disengage on brake", disengage_on_brake=disengage_on_brake):
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, pause_lateral_on_brake)
self.assertEqual(enable_mads and pause_lateral_on_brake, self.safety.get_pause_lateral_on_brake())
self.safety.set_mads_params(enable_mads, disengage_on_brake)
self.assertEqual(enable_mads and disengage_on_brake, self.safety.get_disengage_lateral_on_brake())
finally:
self._mads_states_cleanup()
def test_pause_lateral_on_brake(self):
def test_disengage_lateral_on_brake(self):
try:
self._mads_states_cleanup()
self.safety.set_mads_params(True, False, True)
self.safety.set_mads_params(True, True)
self._rx(self._user_brake_msg(False))
self.safety.set_controls_requested_lat(True)
@@ -181,10 +181,10 @@ class MadsSafetyTestBase(unittest.TestCase):
finally:
self._mads_states_cleanup()
def test_no_pause_lateral_on_brake(self):
def test_no_disengage_lateral_on_brake(self):
try:
self._mads_states_cleanup()
self.safety.set_mads_params(True, False, False)
self.safety.set_mads_params(True, False)
self._rx(self._user_brake_msg(False))
self.safety.set_controls_requested_lat(True)
@@ -204,50 +204,50 @@ class MadsSafetyTestBase(unittest.TestCase):
try:
for enable_mads in (True, False):
with self.subTest("enable_mads", enable_mads=enable_mads):
for pause_lateral_on_brake in (True, False):
with self.subTest("pause_lateral_on_brake", pause_lateral_on_brake=pause_lateral_on_brake):
for disengage_lateral_on_brake in (True, False):
with self.subTest("disengage_lateral_on_brake", disengage_lateral_on_brake=disengage_lateral_on_brake):
with self.subTest("mads_button"):
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, pause_lateral_on_brake)
self.safety.set_mads_params(enable_mads, disengage_lateral_on_brake)
# Brake press rising edge
self._rx(self._user_brake_msg(True))
self._rx(self._lkas_button_msg(True))
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads and not pause_lateral_on_brake, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat())
# Continuous braking after the first frame of brake press rising edge
self.assertEqual(enable_mads and not pause_lateral_on_brake, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat())
for _ in range(400):
self._rx(self._user_brake_msg(True))
self.assertEqual(enable_mads and not pause_lateral_on_brake, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat())
with self.subTest("acc_main_on"):
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, pause_lateral_on_brake)
self.safety.set_mads_params(enable_mads, disengage_lateral_on_brake)
# Brake press rising edge
self._rx(self._user_brake_msg(True))
self.safety.set_acc_main_on(True)
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads and not pause_lateral_on_brake, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat())
# Continuous braking after the first frame of brake press rising edge
self.assertEqual(enable_mads and not pause_lateral_on_brake, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat())
for _ in range(400):
self._rx(self._user_brake_msg(True))
self.assertEqual(enable_mads and not pause_lateral_on_brake, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat())
finally:
self._mads_states_cleanup()
def test_pause_lateral_on_brake_with_pressed_and_released(self):
def test_disengage_lateral_on_brake_with_pressed_and_released(self):
try:
for enable_mads in (True, False):
with self.subTest("enable_mads", enable_mads=enable_mads):
for pause_lateral_on_brake in (True, False):
with self.subTest("pause_lateral_on_brake", pause_lateral_on_brake=pause_lateral_on_brake):
for disengage_lateral_on_brake in (True, False):
with self.subTest("disengage_lateral_on_brake", disengage_lateral_on_brake=disengage_lateral_on_brake):
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, pause_lateral_on_brake)
self.safety.set_mads_params(enable_mads, disengage_lateral_on_brake)
# Set controls_allowed_lat rising edge
self.safety.set_controls_requested_lat(True)
@@ -257,7 +257,7 @@ class MadsSafetyTestBase(unittest.TestCase):
# User brake press, validate controls_allowed_lat is false
self._rx(self._user_brake_msg(True))
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads and not pause_lateral_on_brake, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat())
# User brake release, validate controls_allowed_lat is true
self._rx(self._user_brake_msg(False))
@@ -266,10 +266,10 @@ class MadsSafetyTestBase(unittest.TestCase):
finally:
self._mads_states_cleanup()
def test_pause_lateral_on_brake_persistent_control_allowed_off(self):
def test_disengage_lateral_on_brake_persistent_control_allowed_off(self):
try:
self._mads_states_cleanup()
self.safety.set_mads_params(True, False, True)
self.safety.set_mads_params(True, True)
self.safety.set_controls_requested_lat(True)
@@ -295,7 +295,7 @@ class MadsSafetyTestBase(unittest.TestCase):
for enable_mads in (True, False):
with self.subTest("enable_mads", enable_mads=enable_mads):
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
self.safety.set_controls_allowed(False)
self._rx(self._speed_msg(0))
@@ -328,7 +328,7 @@ class MadsSafetyTestBase(unittest.TestCase):
for enable_mads in (True, False):
with self.subTest("enable_mads", enable_mads=enable_mads):
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
self._rx(self._lkas_button_msg(True))
self._rx(self._speed_msg(0))
@@ -358,7 +358,7 @@ class MadsSafetyTestBase(unittest.TestCase):
"""
try:
self._mads_states_cleanup()
self.safety.set_mads_params(True, False, True) # enable MADS with disengage on brake
self.safety.set_mads_params(True, True) # enable MADS with disengage on brake
# Initial state
self.safety.set_controls_allowed_lat(True)
@@ -393,7 +393,7 @@ class MadsSafetyTestBase(unittest.TestCase):
"""
try:
self._mads_states_cleanup()
self.safety.set_mads_params(True, False, True) # enable MADS with disengage on brake
self.safety.set_mads_params(True, True) # enable MADS with disengage on brake
# Initial state - enable with ACC main
self.safety.set_acc_main_on(True)

6
opendbc/safety/tests/safety_replay/replay_drive.py
View File

@@ -32,13 +32,11 @@ def replay_drive(msgs, safety_mode, param, alternative_experience, param_sp):
safety.set_alternative_experience(alternative_experience)
_enable_mads = bool(alternative_experience & ALTERNATIVE_EXPERIENCE.ENABLE_MADS)
_disengage_lateral_on_brake = bool(alternative_experience & ALTERNATIVE_EXPERIENCE.MADS_DISENGAGE_LATERAL_ON_BRAKE)
_pause_lateral_on_brake = bool(alternative_experience & ALTERNATIVE_EXPERIENCE.MADS_PAUSE_LATERAL_ON_BRAKE)
safety.set_mads_params(_enable_mads, _disengage_lateral_on_brake, _pause_lateral_on_brake)
_disengage_lateral_on_brake = bool(alternative_experience & ALTERNATIVE_EXPERIENCE.DISENGAGE_LATERAL_ON_BRAKE)
safety.set_mads_params(_enable_mads, _disengage_lateral_on_brake)
print("alternative experience:")
print(f" enable mads: {_enable_mads}")
print(f" disengage lateral on brake: {_disengage_lateral_on_brake}")
print(f" pause lateral on brake: {_pause_lateral_on_brake}")
init_segment(safety, msgs, safety_mode, param)

2
opendbc/safety/tests/test_ford.py
View File

@@ -400,7 +400,7 @@ class TestFordSafetyBase(common.PandaCarSafetyTest):
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.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
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())
finally:

2
opendbc/safety/tests/test_honda.py
View File

@@ -253,7 +253,7 @@ class HondaBase(common.PandaCarSafetyTest):
for enable_mads in (True, False):
with self.subTest("enable_mads", mads_enabled=enable_mads):
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
# Verify initial state
self._rx(self._lkas_button_msg(False, 0))

2
opendbc/safety/tests/test_hyundai.py
View File

@@ -179,7 +179,7 @@ class TestHyundaiSafety(HyundaiButtonBase, common.PandaCarSafetyTest, common.Dri
self.safety.set_safety_hooks(default_safety_mode, default_safety_param)
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
self.assertEqual(enable_mads, self.safety.get_enable_mads())
self._rx(self._lkas_button_msg(True))

2
opendbc/safety/tests/test_subaru.py
View File

@@ -115,7 +115,7 @@ class TestSubaruSafetyBase(common.PandaCarSafetyTest):
for mads_button_press in range(4):
with self.subTest("mads_button_press", button_state=mads_button_press):
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, False, False)
self.safety.set_mads_params(enable_mads, False)
self._rx(self._lkas_button_msg(False, mads_button_press))
self.assertEqual(enable_mads and mads_button_press in range(1, 4),