MADS: improvements on the state machine

[devtekve]

No description provided.

[devtekve]

@sunnyhaibin fyi

[devtekve]

MADS Lateral Control: All Critical States

1. Final Output Truth Table (mads_is_lateral_control_allowed_by_mads)

system_enabled	controls_allowed_lat	Is lat allowed?
false	false	NO - System disabled
false	true	NO - Invalid state, cannot occur
true	false	NO - Controls not permitted
true	true	YES - Active lateral control

2. Control Permission State (m_can_allow_controls_lat) - When system_enabled = true

current_disengage.reason	is_braking	disengage_lateral_on_brake	Is lat allowed?
NONE	*	*	YES - No disengagement active
LAG	*	*	YES - LAG doesn't prevent control
BUTTON	*	*	YES - BUTTON doesn't prevent control
ACC_MAIN_OFF	*	*	YES - ACC_MAIN_OFF doesn't prevent control
BRAKE	true	true	NO - Active brake blocking
BRAKE	false	true	YES - Brake released
BRAKE	true	false	YES - Brake blocking disabled
BRAKE	false	false	YES - Both brake conditions inactive

3. Button Control Request States and Transitions

From m_update_button_state():

if (button.transition == MADS_EDGE_RISING) {
  const bool acc_active = *m_mads_state.acc_main.current;
  m_mads_state.controls_requested_lat = !m_mads_state.controls_allowed_lat;
  
  if (!m_mads_state.controls_requested_lat && !acc_active) {
    mads_exit_controls(MADS_DISENGAGE_REASON_BUTTON);
  }
}

button_transition	controls_allowed_lat	acc_active	Result	Action Taken
NOT_RISING_EDGE	any	any	NO CHANGE	None - Only rising edge matters
RISING_EDGE	false	true	REQUEST	controls_requested_lat = true
RISING_EDGE	false	false	REQUEST	controls_requested_lat = true
RISING_EDGE	true	true	NO CHANGE	controls_requested_lat = false, but no disengage
RISING_EDGE	true	false	DISENGAGE	controls_requested_lat = false AND mads_exit_controls(BUTTON)

4. ACC Main State Changes

From m_update_binary_state():

static void m_update_binary_state(BinaryStateTracking *state) {
  EdgeTransition transition = m_get_edge_transition(*state->current, state->previous);
  if (transition == MADS_EDGE_RISING) {
    m_mads_state.controls_requested_lat = true;
  } else if (transition == MADS_EDGE_FALLING) {
    m_mads_state.controls_requested_lat = false;
    mads_exit_controls(MADS_DISENGAGE_REASON_ACC_MAIN_OFF);
  }

  state->previous = *state->current;
}

acc_main_transition	controls_allowed_lat	Result	Action Taken
NO_CHANGE	any	NO CHANGE	None - Only edges matter
RISING_EDGE	false	REQUEST	controls_requested_lat = true
RISING_EDGE	true	NO CHANGE	controls_requested_lat = true (redundant)
FALLING_EDGE	false	NO CHANGE	controls_requested_lat = false
FALLING_EDGE	true	DISENGAGE	controls_requested_lat = false AND mads_exit_controls(ACC_MAIN_OFF)

5. Complete Control Request Resolution

controls_requested_lat	m_can_allow_controls_lat	already_allowed	Will controls be allowed?
false	false	false	NO - Not requested
false	true	false	NO - Not requested
true	false	false	NO - Cannot allow yet
true	true	false	YES - New control granted
false	any	true	NO - Will lose control
true	false	true	YES - Maintains existing control
true	true	true	YES - Maintains control

6. Brake Impact States

is_braking	was_braking	vehicle_moving	disengage_lateral_on_brake	Will controls be lost?
false	any	any	any	NO
true	true	false	true	NO - Already handled
true	true	true	true	YES - Moving with brake
true	false	any	true	YES - New brake press
true	any	any	false	NO - Brake blocking disabled

7. Disengage Reason State Machine

7.1 Setting Disengage Reasons

if (m_mads_state.controls_allowed_lat) {
  m_mads_state.previous_disengage = m_mads_state.current_disengage;
  m_mads_state.current_disengage.reason = reason;
  m_mads_state.controls_allowed_lat = false;
}

7.2 Clearing Disengage Reasons

if (m_mads_state.controls_requested_lat && !m_mads_state.controls_allowed_lat && m_can_allow_controls_lat()) {
  m_mads_state.controls_allowed_lat = true;
  m_mads_state.previous_disengage = m_mads_state.current_disengage;
  m_mads_state.current_disengage.reason = MADS_DISENGAGE_REASON_NONE;
}

7.3 Disengage Reason Transitions

Current Reason	Event	New Reason	Controls Allowed?	Notes
NONE	Button press when !acc_active	BUTTON	NO	From m_update_button_state
NONE	ACC main off	ACC_MAIN_OFF	NO	From m_update_binary_state
NONE	Brake press when disengage_lateral_on_brake=true	BRAKE	NO	From m_mads_check_braking
NONE	Brake press when disengage_lateral_on_brake=false	NONE	YES	Braking doesn't affect control
BRAKE	Brake release when disengage_lateral_on_brake=true	NONE	YES	Auto-restores when brake released
BUTTON	ACC turns on + new request	NONE	YES	Via m_can_allow_controls_lat
ACC_MAIN_OFF	ACC turns on + new request	NONE	YES	Via m_can_allow_controls_lat
Any	New disengage event	New Reason	NO	Previous reason saved

Key behavior differences:

1. With disengage_lateral_on_brake = false:

   • Braking never causes disengagement
   • BRAKE reason never gets set

2. With disengage_lateral_on_brake = true:

   • Braking causes immediate disengagement
   • Controls auto-restore when brake is released
   • No explicit re-request needed

8. mads_state_update() Full Sequence

8.1 Function Input Parameters

void mads_state_update(
    const bool *op_vehicle_moving,
    const bool *op_acc_main,
    bool is_braking,
    bool cruise_engaged  // Currently unused
)

8.2 Initialization Steps

m_mads_state.is_vehicle_moving_ptr = op_vehicle_moving;
m_mads_state.acc_main.current = op_acc_main;
m_mads_state.main_button.current = &main_button_press;
m_mads_state.lkas_button.current = &lkas_button_press;

8.3 Button State Flag Updates

if (!(m_mads_state.state_flags & MADS_STATE_FLAG_MAIN_BUTTON_AVAILABLE) && 
    (main_button_press != MADS_BUTTON_UNAVAILABLE)) {
    m_mads_state.state_flags |= MADS_STATE_FLAG_MAIN_BUTTON_AVAILABLE;
}

if (!(m_mads_state.state_flags & MADS_STATE_FLAG_LKAS_BUTTON_AVAILABLE) && 
    (lkas_button_press != MADS_BUTTON_UNAVAILABLE)) {
    m_mads_state.state_flags |= MADS_STATE_FLAG_LKAS_BUTTON_AVAILABLE;
}

8.4 Update Sequence (in order of execution)

1. Button State Updates:

m_update_button_state(&m_mads_state.main_button);
m_update_button_state(&m_mads_state.lkas_button);

Effects:

• Can set controls_requested_lat true/false
• Can trigger BUTTON disengage
• Button transitions only processed if RISING edge

2. ACC Main Update:

m_update_binary_state(&m_mads_state.acc_main);

Effects:

• RISING edge sets controls_requested_lat true
• FALLING edge forces controls_requested_lat false and disengages with ACC_MAIN_OFF

3. Brake Check:

m_mads_check_braking(is_braking);

Effects:

• If disengage_lateral_on_brake true:
  • New brake OR moving vehicle causes BRAKE disengage
• Updates is_braking state

4. Control Permission Check:

m_mads_try_allow_controls_lat();

Effects:

• If controls requested AND not allowed AND can allow:
  • Grants control permission
  • Clears disengage reason to NONE

8.5 Critical Timing Dependencies

1. Button handling occurs first:

   • Can request OR cancel controls before other checks
   • Button disengage takes precedence over ACC state

2. ACC main processed second:

   • Can override button's control request
   • FALLING edge always disengages regardless of button state

3. Brake check third:

   • Can disengage even if buttons/ACC just requested control
   • Only matters if disengage_lateral_on_brake true

4. Final permission check last:

   • Only proceeds if all previous steps allow it
   • Auto-restores control if conditions permit

This sequence means:

• ACC main off always wins over button requests
• Brake (if enabled) can override both button and ACC
• Control permission only granted if all conditions remain satisfied after all updates

[sunnyhaibin]

Updated version in #52