Skip to content

MADS safety: enable heartbeat check and dedicated states #113

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
sunnyhaibin merged 2 commits into from
Apr 14, 2026
Merged

MADS safety: enable heartbeat check and dedicated states #113

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
cfbaa8e
init
sunnyhaibin Apr 13, 2026
6dd5b47
rename
sunnyhaibin Apr 14, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions board/health.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@ struct __attribute__((packed)) health_t {
uint16_t sbu2_voltage_mV;
uint8_t som_reset_triggered;
uint16_t sound_output_level_pkt;
uint8_t controls_allowed_lateral_pkt;
uint8_t controls_allowed_longitudinal_pkt;
};

typedef struct __attribute__((packed)) {
Expand Down
5 changes: 4 additions & 1 deletion board/main_comms.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,9 @@ static int get_health_pkt(void *dat) {

health->sound_output_level_pkt = sound_output_level;

health->controls_allowed_lateral_pkt = controls_allowed || controls_allowed_lateral;
health->controls_allowed_longitudinal_pkt = controls_allowed;

return sizeof(*health);
}

Expand Down Expand Up @@ -300,7 +303,7 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
heartbeat_lost = false;
heartbeat_disabled = false;
heartbeat_engaged = (req->param1 == 1U);
heartbeat_engaged_mads = true; // FIXME-SP: Implement proper heartbeat check from sunnypilot
heartbeat_engaged_mads = (req->param2 == 1U);
break;
}
// **** 0xf6: set siren enabled
Expand Down
2 changes: 2 additions & 0 deletions python/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -543,6 +543,8 @@ def health(self):
"sbu2_voltage_mV": a[23],
"som_reset_triggered": a[24],
"sound_output_level": a[25],
"controls_allowed_lateral": a[26],
"controls_allowed_longitudinal": a[27],
}

@ensure_health_packet_version
Expand Down
98 changes: 49 additions & 49 deletions tests/safety/mads_common.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,8 @@ def _acc_state_msg(self, enabled):

def _mads_states_cleanup(self):
self.safety.set_mads_button_press(-1)
self.safety.set_controls_allowed_lat(False)
self.safety.set_controls_requested_lat(False)
self.safety.set_controls_allowed_lateral(False)
self.safety.set_controls_requested_lateral(False)
self.safety.set_acc_main_on(False)
self.safety.set_mads_params(False, False)
self.safety.set_heartbeat_engaged_mads(True)
Expand All @@ -37,7 +37,7 @@ def test_enable_control_allowed_with_mads_button(self):
self._rx(self._speed_msg(0))
self._rx(self._lkas_button_msg(False))
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
finally:
self._mads_states_cleanup()

Expand All @@ -55,7 +55,7 @@ def test_enable_control_allowed_with_manual_acc_main_on_state(self):
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())
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
finally:
self._mads_states_cleanup()

Expand All @@ -70,7 +70,7 @@ def test_enable_control_allowed_with_manual_mads_button_state(self):

self.safety.set_mads_button_press(mads_button_press)
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads and mads_button_press == 1, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and mads_button_press == 1, self.safety.get_controls_allowed_lateral())
finally:
self._mads_states_cleanup()

Expand All @@ -88,21 +88,21 @@ def test_enable_control_allowed_from_acc_main_on(self):
self.safety.set_acc_main_on(acc_main_on)
self._rx(self._speed_msg(0))
expected_lat = enable_mads and acc_main_on
self.assertEqual(expected_lat, self.safety.get_controls_allowed_lat(),
self.assertEqual(expected_lat, self.safety.get_controls_allowed_lateral(),
f"Expected lat: [{expected_lat}] when acc_main_on goes to [{acc_main_on}]")

# Test transition to opposite state
self.safety.set_acc_main_on(not acc_main_on)
self._rx(self._speed_msg(0))
expected_lat = enable_mads and not acc_main_on
self.assertEqual(expected_lat, self.safety.get_controls_allowed_lat(),
self.assertEqual(expected_lat, self.safety.get_controls_allowed_lateral(),
f"Expected lat: [{expected_lat}] when acc_main_on goes from [{acc_main_on}] to [{not acc_main_on}]")

# Test transition back to initial state
self.safety.set_acc_main_on(acc_main_on)
self._rx(self._speed_msg(0))
expected_lat = enable_mads and acc_main_on
self.assertEqual(expected_lat, self.safety.get_controls_allowed_lat(),
self.assertEqual(expected_lat, self.safety.get_controls_allowed_lateral(),
f"Expected lat: [{expected_lat}] when acc_main_on goes from [{not acc_main_on}] to [{acc_main_on}]")
finally:
self._mads_states_cleanup()
Expand All @@ -116,11 +116,11 @@ def test_mads_with_acc_main_on(self):

self.safety.set_acc_main_on(True)
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())

self.safety.set_acc_main_on(False)
self._rx(self._speed_msg(0))
self.assertFalse(self.safety.get_controls_allowed_lat())
self.assertFalse(self.safety.get_controls_allowed_lateral())
finally:
self._mads_states_cleanup()

Expand All @@ -142,15 +142,15 @@ def test_disengage_lateral_on_brake(self):
self.safety.set_mads_params(True, True)

self._rx(self._user_brake_msg(False))
self.safety.set_controls_requested_lat(True)
self.safety.set_controls_allowed_lat(True)
self.safety.set_controls_requested_lateral(True)
self.safety.set_controls_allowed_lateral(True)

self._rx(self._user_brake_msg(True))
# Test we pause lateral
self.assertFalse(self.safety.get_controls_allowed_lat())
self.assertFalse(self.safety.get_controls_allowed_lateral())
# Make sure we can re-gain lateral actuation
self._rx(self._user_brake_msg(False))
self.assertTrue(self.safety.get_controls_allowed_lat())
self.assertTrue(self.safety.get_controls_allowed_lateral())
finally:
self._mads_states_cleanup()

Expand All @@ -160,11 +160,11 @@ def test_no_disengage_lateral_on_brake(self):
self.safety.set_mads_params(True, False)

self._rx(self._user_brake_msg(False))
self.safety.set_controls_requested_lat(True)
self.safety.set_controls_allowed_lat(True)
self.safety.set_controls_requested_lateral(True)
self.safety.set_controls_allowed_lateral(True)

self._rx(self._user_brake_msg(True))
self.assertTrue(self.safety.get_controls_allowed_lat())
self.assertTrue(self.safety.get_controls_allowed_lateral())
finally:
self._mads_states_cleanup()

Expand All @@ -187,13 +187,13 @@ def test_engage_with_brake_pressed(self):
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 disengage_lateral_on_brake, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lateral())

# Continuous braking after the first frame of brake press rising edge
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lateral())
for _ in range(400):
self._rx(self._user_brake_msg(True))
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lateral())

with self.subTest("acc_main_on"):
self._mads_states_cleanup()
Expand All @@ -203,13 +203,13 @@ def test_engage_with_brake_pressed(self):
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 disengage_lateral_on_brake, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lateral())

# Continuous braking after the first frame of brake press rising edge
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lateral())
for _ in range(400):
self._rx(self._user_brake_msg(True))
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lateral())
finally:
self._mads_states_cleanup()

Expand All @@ -222,20 +222,20 @@ def test_disengage_lateral_on_brake_with_pressed_and_released(self):
self._mads_states_cleanup()
self.safety.set_mads_params(enable_mads, disengage_lateral_on_brake)

# Set controls_allowed_lat rising edge
self.safety.set_controls_requested_lat(True)
# Set controls_allowed_lateral rising edge
self.safety.set_controls_requested_lateral(True)
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())

# User brake press, validate controls_allowed_lat is false
# User brake press, validate controls_allowed_lateral is false
self._rx(self._user_brake_msg(True))
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads and not disengage_lateral_on_brake, self.safety.get_controls_allowed_lateral())

# User brake release, validate controls_allowed_lat is true
# User brake release, validate controls_allowed_lateral is true
self._rx(self._user_brake_msg(False))
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())
finally:
self._mads_states_cleanup()

Expand All @@ -244,22 +244,22 @@ def test_disengage_lateral_on_brake_persistent_control_allowed_off(self):
self._mads_states_cleanup()
self.safety.set_mads_params(True, True)

self.safety.set_controls_requested_lat(True)
self.safety.set_controls_requested_lateral(True)

# Vehicle moving, validate controls_allowed_lat is true
# Vehicle moving, validate controls_allowed_lateral is true
for _ in range(10):
self._rx(self._speed_msg(10))
self.assertTrue(self.safety.get_controls_allowed_lat())
self.assertTrue(self.safety.get_controls_allowed_lateral())

# User braked, vehicle slowed down in 10 frames, then stopped for 10 frames
# Validate controls_allowed_lat is false
# Validate controls_allowed_lateral is false
self._rx(self._user_brake_msg(True))
for _ in range(10):
self._rx(self._speed_msg(5))
self.assertFalse(self.safety.get_controls_allowed_lat())
self.assertFalse(self.safety.get_controls_allowed_lateral())
for _ in range(10):
self._rx(self._speed_msg(0))
self.assertFalse(self.safety.get_controls_allowed_lat())
self.assertFalse(self.safety.get_controls_allowed_lateral())
finally:
self._mads_states_cleanup()

Expand Down Expand Up @@ -307,15 +307,15 @@ def test_enable_control_allowed_with_mads_button_and_disable_with_main_cruise(se
self._rx(self._speed_msg(0))
self._rx(self._lkas_button_msg(False))
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())

self._rx(self._acc_state_msg(True))
self._rx(self._speed_msg(0))
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lat())
self.assertEqual(enable_mads, self.safety.get_controls_allowed_lateral())

self._rx(self._acc_state_msg(False))
self._rx(self._speed_msg(0))
self.assertFalse(self.safety.get_controls_allowed_lat())
self.assertFalse(self.safety.get_controls_allowed_lateral())
finally:
self._mads_states_cleanup()

Expand All @@ -334,22 +334,22 @@ def test_brake_disengage_with_control_request(self):
self.safety.set_mads_params(True, True) # enable MADS with disengage on brake

# Initial state
self.safety.set_controls_allowed_lat(True)
self.safety.set_controls_allowed_lateral(True)
self._rx(self._speed_msg(0))
self.assertTrue(self.safety.get_controls_allowed_lat())
self.assertTrue(self.safety.get_controls_allowed_lateral())

# Brake press disengages lateral
self._rx(self._user_brake_msg(True))
self.assertFalse(self.safety.get_controls_allowed_lat())
self.assertFalse(self.safety.get_controls_allowed_lateral())

# Request controls while braking
self.safety.set_controls_requested_lat(True)
self.assertFalse(self.safety.get_controls_allowed_lat())
self.safety.set_controls_requested_lateral(True)
self.assertFalse(self.safety.get_controls_allowed_lateral())

# Release brake - should enable since controls were requested
self._rx(self._user_brake_msg(False))
self._rx(self._speed_msg(0))
self.assertTrue(self.safety.get_controls_allowed_lat())
self.assertTrue(self.safety.get_controls_allowed_lateral())

finally:
self._mads_states_cleanup()
Expand All @@ -371,21 +371,21 @@ def test_brake_disengage_with_acc_main_off(self):
# Initial state - enable with ACC main
self.safety.set_acc_main_on(True)
self._rx(self._speed_msg(0))
self.assertTrue(self.safety.get_controls_allowed_lat())
self.assertTrue(self.safety.get_controls_allowed_lateral())

# Brake press disengages lateral
self._rx(self._user_brake_msg(True))
self.assertFalse(self.safety.get_controls_allowed_lat())
self.assertFalse(self.safety.get_controls_allowed_lateral())

# Turn ACC main off while braking
self.safety.set_acc_main_on(False)
self._rx(self._speed_msg(0))
self.assertFalse(self.safety.get_controls_allowed_lat())
self.assertFalse(self.safety.get_controls_allowed_lateral())

# Release brake - should remain disabled since ACC main is off
self._rx(self._user_brake_msg(False))
self._rx(self._speed_msg(0))
self.assertFalse(self.safety.get_controls_allowed_lat())
self.assertFalse(self.safety.get_controls_allowed_lateral())

finally:
self._mads_states_cleanup()
Loading