safety/mads: reset heartbeat mismatch counter so a re-engage is not revoked

[Geomglot]

Summary

heartbeat_engaged_mads_mismatches is never cleared on the two paths that should clear it, which makes a MADS re-engage near a heartbeat-mismatch tick get revoked. There are two halves to this race, one commit each. Both are general MADS bugs affecting every brand that uses the heartbeat engaged check, not brand-specific.

Half 1 — re-engage lands after the mismatch exit (commit 1)

mads_exit_controls does not reset the counter, so a heartbeat-mismatch exit leaves it saturated at 3. The next re-engage that lands between two 1 Hz firmware ticks — before heartbeat_engaged_mads catches up — is revoked on its very first mads_heartbeat_engaged_check() (3 -> 4 >= 3), instead of getting a fresh 3-tick grace window.

Fix: one line at the end of mads_exit_controls:

heartbeat_engaged_mads_mismatches = 0U;

Half 2 — re-engage lands before the mismatch exit (commit 2)

This case is worse: it leaves lateral permanently revoked rather than briefly.

When a re-engage arrives while controls_allowed_lateral is still set, the grant block's !controls_allowed_lateral guard skips, so controls_requested_lateral is never consumed and just sits pending. The next heartbeat tick then fires mads_exit_controls(), which drops both controls_allowed_lateral and that pending request. No edge remains that can re-request lateral, so the safety stays lateral-off while openpilot MADS is enabled. openpilot's own watchdog (lateral_mismatch_counter >= 200 in mads.py) then immediate-disables 2 s later with a red "Controls Mismatch: Lateral".

Fix: reset the counter on the rising edge of controls_requested_lateral, mirroring the existing guard in safety.h:

// reset mismatches on rising edge of controls_allowed to avoid rare race condition
if (controls_allowed && !controls_allowed_prev) {
  heartbeat_engaged_mismatches = 0;
}

Neither fix weakens the watchdog: if openpilot is genuinely not engaged, the counter re-accumulates from zero and the safety revokes 3 ticks later.

How it was found

Both halves were observed in the field on Rivian, where the MADS disengage gesture (stalk past detent) disengages MADS on the openpilot side without a corresponding safety-side exit — so the mismatch window is entered on every such disengage, and a driver re-engaging within ~3 s hits it.

Half 1 is destructive on Rivian: the in-flight lateral TX frames sent during the one-tick revoke are safety-rejected, putting counter gaps on the bus that latch an EPAS AngleControlCntr fault (EAC fault + ToiFlt -> steerFaultPermanent, "TAKE CONTROL IMMEDIATELY").

Frame-by-frame replay of the field route c17ea97dc5472650/00000006 seg 3 through the compiled safety model:

	TX mismatches vs field	Behaviour in the fault window (t≈222 s)
Before (no reset)	0 (reproduces field firmware bit-exactly, 117,595 frames)	re-engage at t=221.95 revoked 0.1 s later at t=222.05 → 8 0x110 frames blocked → EPAS fault
After (reset)	8 on 0x110 (field_blocked=True, replay_blocked=False)	re-engage survives; the 8 frames pass → fault prevented

Half 2 was found later in route 4440a486580ed7c6/00000059 seg 0, on a build that already had the half-1 fix. Log timeline:

t (s)	Event
21.26	MADS button → openpilot MADS on; safety grants controls_allowed_lateral
21.32	stalk past detent → openpilot MADS off; safety keeps lateral granted
~21.4–23.2	mismatch counter ticks 1, 2
23.28	driver re-presses MADS button → openpilot MADS on; request pends, is not consumed
~23.28	3rd tick → mads_exit_controls() drops lateral and the pending request
25.28	openpilot lateral_mismatch_counter hits 200 → controlsMismatchLateral immediate disable

Testing

Two regression tests in mads_common.py, both running for every MADS brand via CarSafetyTest, each verified to fail without its fix and pass with it:

• test_heartbeat_engaged_mads_reengage_after_mismatch — saturates the counter with a mismatch exit, re-engages, asserts the next tick does not instantly revoke.
• test_heartbeat_engaged_mads_reengage_before_mismatch_exit — engages, ticks the counter to 2, re-engages while lateral is still granted, asserts the 3rd tick does not revoke. Drives the engage through set_mads_button_press() rather than _lkas_button_msg(): Subaru's mads_button_press is sticky (set on HUD state 1-3, never cleared), so it cannot produce the second rising edge this test needs.

Full safety suite green across all brands: 7578 passed, 3126 skipped. MISRA was not run locally (cppcheck has no Darwin/x86_64 build) — relying on CI for that.

---

🤖 Generated with Claude Code