rust_gnc/control/
stabilizer.rs

1//! # Flight Stabilization Orchestrator
2//! 
3//! This module implements the `Stabilizer`, the central coordinator of the flight 
4//! control stack. It manages the lifecycle of the aircraft (Arming/Disarming) and 
5//! executes the primary control loop (Guidance -> Navigation -> Control).
6use crate::filters::{Filter, InertialInput};
7use crate::control::{AxisProcessor, FailsafeLevel, FailsafeMonitor, Mixer, PidConfig, PidController};
8use crate::units::{Radians, Attitude};
9
10/// Defines the operational safety state of the vehicle.
11#[derive(PartialEq, Debug)]
12pub enum ArmingState {
13    Disarmed,
14    Armed,
15}
16
17/// A container for the three primary rotational control axes.
18pub struct AttitudeController<F> where F: Filter<Input = InertialInput, Output = Radians> {
19    pub roll_processor: AxisProcessor<F>,
20    pub pitch_processor: AxisProcessor<F>,
21    pub yaw_processor: AxisProcessor<F>,
22}
23
24/// The top-level interface for the flight control system.
25/// 
26/// The `Stabilizer` manages state estimation and PID calculation for all axes, 
27/// eventually producing motor signals for the mixer.
28pub struct Stabilizer<F, M>
29    where 
30        F: Filter<Input = InertialInput, Output = Radians>,
31        M: Mixer 
32{
33    pub attitude_controller: AttitudeController<F>,
34    pub arming_state: ArmingState,
35    pub mixer: M,
36    pub failsafe_monitor: FailsafeMonitor,
37}
38
39impl<F, M> Stabilizer<F, M> where 
40    F: Filter<Input = InertialInput, Output = Radians>, 
41    M: Mixer 
42{
43    pub fn new(
44        roll_cfg: PidConfig,
45        pitch_cfg: PidConfig,
46        yaw_cfg: PidConfig,
47        alpha: f32,
48        mixer: M,
49        failsafe_timeout: f32
50    ) -> Self {
51        Self {
52            arming_state: ArmingState::Disarmed,
53            attitude_controller: AttitudeController {
54                roll_processor: AxisProcessor::new(F::new(alpha), PidController::new(roll_cfg)),
55                pitch_processor: AxisProcessor::new(F::new(alpha), PidController::new(pitch_cfg)),
56                yaw_processor: AxisProcessor::new(F::new(alpha), PidController::new(yaw_cfg)),
57            },
58            mixer: mixer,
59            failsafe_monitor: FailsafeMonitor::new(failsafe_timeout),
60        }
61    }
62
63    /// Transitions the flight state and handles critical hardware resets.
64    /// 
65    /// When disarming, all PID integrals and filters are cleared to prevent 
66    /// "jump-on-arm" behavior caused by stale data accumulation (I-term windup).
67    pub fn set_armed(&mut self, state: ArmingState) {
68        self.arming_state = state;
69        if self.arming_state == ArmingState::Disarmed {
70            // CRITICAL: Reset all PID integrals and Filter states
71            // This prevents "jump-on-arm" behavior.
72            self.attitude_controller.pitch_processor.pid.reset();
73            self.attitude_controller.roll_processor.pid.reset();
74            self.attitude_controller.yaw_processor.pid.reset();
75            
76            self.attitude_controller.pitch_processor.filter.reset();
77            self.attitude_controller.roll_processor.filter.reset();
78            self.attitude_controller.yaw_processor.filter.reset();
79        }
80    }
81
82    /// Executes a single iteration of the flight control loop.
83    /// 
84    /// This is the "Heartbeat" of the aircraft, typically running at 400Hz to 1kHz.
85    /// It coordinates safety checks, sensor fusion, and actuator mixing.
86    /// 
87    /// ### Logic Flow
88    /// 1. **Failsafe Evaluation**: Checks the system health/heartbeat against `current_time`.
89    /// 2. **Command Override**: If a failsafe is active, pilot inputs are overridden with 
90    ///    emergency values (e.g., leveling out and reducing throttle).
91    /// 3. **Arming Interlock**: Ensures zero motor output if the aircraft is disarmed.
92    /// 4. **Axis Processing**: Filters raw IMU data and calculates PID corrections for Roll/Pitch.
93    /// 5. **Heading Control**: Calculates the shortest-path error for Yaw (circular normalization).
94    /// 6. **Actuator Mixing**: Maps 3-axis demands and throttle into airframe-specific motor signals.
95    ///
96    /// ### Returns
97    /// Returns `M::Output`, which for a Quadcopter is `QuadMotorSignals`.
98    pub fn tick(
99        &mut self, 
100        roll_input: InertialInput, 
101        pitch_input: InertialInput, 
102        yaw_input: InertialInput,
103        target: Attitude,
104        throttle: f32,
105        dt: f32,
106        current_time: f32
107    ) -> M:: Output {
108
109        let failsafe_level = self.failsafe_monitor.check(current_time);
110
111        let (effective_throttle, effective_target) = match failsafe_level {
112            FailsafeLevel::None => (throttle, target),
113            FailsafeLevel::Land => (throttle * 0.5, Attitude::default()), // 50% power & Level
114            FailsafeLevel::Kill => (0.0, Attitude::default()),            // Cut power
115        };
116
117        // Safeguard: If disarmed, return zero motor outputs
118        if self.arming_state == ArmingState::Disarmed {
119            return self.mixer.mix(0.0, 0.0, 0.0, 0.0);
120        }
121
122        // Process Roll and Pitch axes
123        let roll_output = self.attitude_controller.roll_processor.process(roll_input, effective_target.roll, dt);
124        let pitch_output = self.attitude_controller.pitch_processor.process(pitch_input, effective_target.pitch, dt);
125        
126        let current_yaw = self.attitude_controller.yaw_processor.filter.update(yaw_input, dt);
127        let yaw_error = current_yaw.shortest_distance_to(effective_target.yaw);
128        let yaw_output = self.attitude_controller.yaw_processor.pid.update_with_error(yaw_error, dt);
129
130        self.mixer.mix(roll_output, pitch_output, yaw_output, effective_throttle)
131    }
132}
rust_gnc/control/stabilizer.rs

rust_gnc/control/
stabilizer.rs
