Control FSM

The Control FSM (control_fsm.c) is active when the vehicle is in the SYSTEM_CONTROL state (i.e., successfully completed the R2D sequence). It manages active driving safety checks and motor control loops.

Flow Diagram

States

State	Description
SYSTEM_CONTROL_COOLING	Recalculates and adjusts the cooling system parameters.
SYSTEM_CONTROL_APPS_VALIDATION	Active when an Accelerator Pedal Position Sensor (APPS) error is detected. Cuts motor power until the error is resolved.
SYSTEM_CONTROL_MOTORS	The normal driving state where the motor control logic is evaluated and executed.

Transitions & Events

• Driver Stop Request: If the driver presses the R2D button while in control mode (stop_request), the Control FSM immediately requests the ECU FSM to transition to SYSTEM_SHUTDOWN.
• APPS Validation: The FSM continuously monitors apps_error (e.g., pedal implausibility). If an error is flagged, it forces the state to SYSTEM_CONTROL_APPS_VALIDATION. It will only return to SYSTEM_CONTROL_MOTORS once the APPS readings are validated and safe.
• Cooling Recycle: A periodic cooling_recalc_requested flag will briefly push the FSM into SYSTEM_CONTROL_COOLING to adjust fans/pumps before returning to SYSTEM_CONTROL_MOTORS.

Step Execution (control_fsm_step)

The step function is called in the main loop and forces a strict execution rate. It averages an exact 3ms interval to step the Unscented Kalman Filter (UKF_Step_Safe()).

For motor control:

• Active Closed-Loop Control: If the IMU/GPS speed-source watchdog has not tripped (speed_source_apps_fallback == 0), GPS speed is >= 0.6 m/s, and motor control is toggled on via the dashboard, the ECU executes the active torque vectoring and slip control algorithms (motor_control_step_safe()).
• Direct APPS Passthrough Fallback: If any of the above conditions are not met (including a watchdog timeout on either IMU or GPS telemetry), the ECU disables active motor control and falls back to Direct APPS passthrough. This routes the driver's throttle inputs directly and equally to both left and right motors, ensuring safe, open-loop operation.