veloxity_core/world/
control.rs

1use super::*;
2
3#[cfg(feature = "runtime-diagnostics")]
4use core::sync::atomic::{AtomicU32, Ordering};
5
6#[cfg(feature = "runtime-diagnostics")]
7macro_rules! diagnostic_counter {
8    ($name:ident) => {
9        #[unsafe(no_mangle)]
10        pub static $name: AtomicU32 = AtomicU32::new(0);
11    };
12}
13
14#[cfg(feature = "runtime-diagnostics")]
15diagnostic_counter!(VELOXITY_DIAG_IMU_TICK_COUNT);
16#[cfg(feature = "runtime-diagnostics")]
17diagnostic_counter!(VELOXITY_DIAG_IMU_TICK_SUM_US);
18#[cfg(feature = "runtime-diagnostics")]
19diagnostic_counter!(VELOXITY_DIAG_IMU_TICK_MAX_US);
20#[cfg(feature = "runtime-diagnostics")]
21diagnostic_counter!(VELOXITY_DIAG_ARMED_IMU_TICK_COUNT);
22#[cfg(feature = "runtime-diagnostics")]
23diagnostic_counter!(VELOXITY_DIAG_ARMED_IMU_TICK_SUM_US);
24#[cfg(feature = "runtime-diagnostics")]
25diagnostic_counter!(VELOXITY_DIAG_ARMED_IMU_TICK_MAX_US);
26#[cfg(feature = "runtime-diagnostics")]
27diagnostic_counter!(VELOXITY_DIAG_POST_IMU_SERVICE_AVAILABLE);
28#[cfg(feature = "runtime-diagnostics")]
29diagnostic_counter!(VELOXITY_DIAG_POST_IMU_BLOCKED_PENDING_IMU);
30#[cfg(feature = "runtime-diagnostics")]
31diagnostic_counter!(VELOXITY_DIAG_POST_IMU_BLOCKED_CONTROL_DUE);
32#[cfg(feature = "runtime-diagnostics")]
33diagnostic_counter!(VELOXITY_DIAG_POST_IMU_BLOCKED_GUARD);
34
35impl<B, E, C, M, CI, PD, R> World<B, E, C, M, CI, PD, R>
36where
37    B: BoardIo,
38    E: Estimator<R>,
39    C: Controller<R, State = E::State> + RcTrimCalibrator,
40    M: crate::mixer::Mixer<R, MixerInput = C::ControlOutput>,
41    M::ActuatorCommands: AsRef<[R]> + Copy,
42    E::State: Copy + Default,
43    CI: CommInterface<B>,
44    PD: PwmDriver<R>,
45    R: FlightFloat,
46{
47    pub fn run_once(&mut self) -> bool {
48        self.run_communication_and_parameter_service_stage();
49        self.run_sensor_ingestion_and_health_stage();
50        let fresh_rc = self.processed_sensors.rc;
51        self.run_rc_command_state_stages(fresh_rc);
52        self.run_control_and_mixing_stage_if_new_imu();
53        self.run_telemetry_stage();
54        self.board.serial_flush();
55        self.board.run_deferred_board_actions();
56        true
57    }
58
59    pub fn set_telemetry_rates(&mut self, telemetry_rates: TelemetryRates) {
60        self.comm.set_telemetry_rates(telemetry_rates);
61    }
62
63    pub fn set_control_loop_rates(&mut self, control_loop_rates: ControlLoopRates) {
64        self.control_loop_rates = control_loop_rates;
65        self.last_control_update_us = self.board.clock_micros();
66    }
67
68    pub fn set_test_pin_3(&mut self, high: bool) {
69        self.board.set_test_pin_3(high);
70    }
71
72    pub fn set_test_pin_2(&mut self, high: bool) {
73        self.board.set_test_pin_2(high);
74    }
75
76    pub fn run_comm_param_sensor_stages(&mut self) {
77        self.run_communication_and_parameter_service_stage();
78        self.run_sensor_ingestion_and_health_stage();
79    }
80
81    pub fn imu_pending(&self) -> bool {
82        self.board.imu_pending()
83    }
84
85    pub fn realtime_scheduler_step(&self) -> RealtimeSchedulerStep {
86        if self.imu_pending() {
87            return RealtimeSchedulerStep::ImuControl;
88        }
89        let now_us = self.board.clock_micros();
90        if self.control_update_can_run_at(now_us) {
91            return RealtimeSchedulerStep::ControlUpdate;
92        }
93        if now_us >= self.next_realtime_service_us
94            && self.realtime_service_has_control_slack(now_us)
95        {
96            RealtimeSchedulerStep::Service
97        } else {
98            RealtimeSchedulerStep::Idle
99        }
100    }
101
102    pub fn run_imu_control_tick(&mut self) -> bool {
103        #[cfg(feature = "pre-control-scope")]
104        self.board.set_test_pin_3(true);
105        let now_us = self.board.clock_micros();
106        self.board.update_imu_sensor(&mut self.raw_sensors);
107        self.process_imu_sensor_after_update();
108        self.record_control_imu_candidate();
109        self.update_sensor_health_and_calibration(now_us);
110        #[cfg(feature = "pre-control-scope")]
111        self.board.set_test_pin_3(false);
112        let ran_control = self.run_control_and_mixing_stage_if_control_due(now_us);
113        if ran_control {
114            self.last_realtime_control_us = self.board.clock_micros();
115        }
116        #[cfg(feature = "runtime-diagnostics")]
117        {
118            let finished_us = self.board.clock_micros();
119            let elapsed_us = finished_us.saturating_sub(now_us).min(u32::MAX as u64) as u32;
120            VELOXITY_DIAG_IMU_TICK_COUNT.fetch_add(1, Ordering::Relaxed);
121            VELOXITY_DIAG_IMU_TICK_SUM_US.fetch_add(elapsed_us, Ordering::Relaxed);
122            VELOXITY_DIAG_IMU_TICK_MAX_US.fetch_max(elapsed_us, Ordering::Relaxed);
123            if self.state.is_armed() {
124                VELOXITY_DIAG_ARMED_IMU_TICK_COUNT.fetch_add(1, Ordering::Relaxed);
125                VELOXITY_DIAG_ARMED_IMU_TICK_SUM_US.fetch_add(elapsed_us, Ordering::Relaxed);
126                VELOXITY_DIAG_ARMED_IMU_TICK_MAX_US.fetch_max(elapsed_us, Ordering::Relaxed);
127            }
128
129            if self.imu_pending() {
130                VELOXITY_DIAG_POST_IMU_BLOCKED_PENDING_IMU.fetch_add(1, Ordering::Relaxed);
131            } else if self.control_update_can_run_at(finished_us) {
132                VELOXITY_DIAG_POST_IMU_BLOCKED_CONTROL_DUE.fetch_add(1, Ordering::Relaxed);
133            } else if !self.realtime_service_has_control_slack(finished_us) {
134                VELOXITY_DIAG_POST_IMU_BLOCKED_GUARD.fetch_add(1, Ordering::Relaxed);
135            } else {
136                VELOXITY_DIAG_POST_IMU_SERVICE_AVAILABLE.fetch_add(1, Ordering::Relaxed);
137            }
138        }
139        ran_control
140    }
141
142    pub fn run_control_update_tick(&mut self) -> bool {
143        let now_us = self.board.clock_micros();
144        let ran_control = self.run_control_and_mixing_stage_if_control_due(now_us);
145        if ran_control {
146            self.last_realtime_control_us = self.board.clock_micros();
147        }
148        ran_control
149    }
150
151    pub fn run_rc_command_state_stages(&mut self, fresh_rc: Option<crate::packets::RcPacket>) {
152        let now_ms = self.board.clock_millis();
153
154        run_rc_command_state(RcCommandStateCtx {
155            now_ms,
156            fresh_rc,
157            rc: &mut self.rc,
158            command: &mut self.command,
159            state: &mut self.state,
160            params: &mut self.params,
161            param_events: Some(&mut self.param_events),
162        });
163        self.run_pwm_output_stage();
164        self.update_board_leds(now_ms);
165    }
166
167    pub fn run_pwm_output_stage(&mut self) -> bool {
168        let channel_output_mask = match self
169            .params
170            .get_by_id(crate::params::ParamId::PARAM_CHANNEL_OUTPUT_MASK)
171        {
172            crate::params::ParamValue::Int(value) => value,
173            _ => 0,
174        };
175        let output_kill_active = self.rc.switch_mapped(crate::rc::Switch::OutputKill)
176            && self.rc.switch_on(crate::rc::Switch::OutputKill);
177        sync_pwm_output_state(PwmSyncCtx {
178            board: &mut self.board,
179            pwm: &mut self.pwm,
180            output: &mut self.pwm_output,
181            output_kill_active,
182            channel_output_mask,
183        })
184        .unwrap_or(false)
185    }
186    pub fn run_control_stages_if_new_imu(&mut self) -> bool {
187        self.run_control_and_mixing_stage_if_new_imu()
188    }
189
190    pub(super) fn control_update_due_at(&self, now_us: u64) -> bool {
191        let rate_hz = self.control_loop_rates.control_hz;
192        if rate_hz == 0 {
193            return false;
194        }
195
196        let interval_us = 1_000_000_u64 / rate_hz as u64;
197        now_us.saturating_sub(self.last_control_update_us) >= interval_us
198    }
199
200    pub(super) fn consume_control_update_deadline(&mut self, now_us: u64) {
201        let rate_hz = self.control_loop_rates.control_hz;
202        if rate_hz == 0 {
203            return;
204        }
205
206        let interval_us = 1_000_000_u64 / rate_hz as u64;
207        let elapsed_intervals = now_us.saturating_sub(self.last_control_update_us) / interval_us;
208        self.last_control_update_us = self
209            .last_control_update_us
210            .saturating_add(elapsed_intervals.saturating_mul(interval_us));
211    }
212
213    pub(super) fn control_update_can_run_at(&self, now_us: u64) -> bool {
214        self.control_update_due_at(now_us) && self.control_imu_accumulator.has_samples()
215    }
216
217    pub(super) fn realtime_service_has_control_slack(&self, now_us: u64) -> bool {
218        let rate_hz = self.control_loop_rates.control_hz;
219        if rate_hz == 0 {
220            return true;
221        }
222        // A fixed-rate control update cannot run without accumulated IMU data.
223        // Do not reserve the nominal deadline's guard window for impossible
224        // work: service can use that time and will stop as soon as a new IMU
225        // interrupt makes control work possible again.
226        if !self.control_imu_accumulator.has_samples() {
227            return true;
228        }
229
230        let interval_us = 1_000_000_u64 / rate_hz as u64;
231        let elapsed_us = now_us.saturating_sub(self.last_control_update_us);
232        elapsed_us
233            .checked_add(REALTIME_SERVICE_MIN_CONTROL_SLACK_US)
234            .is_some_and(|required_us| required_us < interval_us)
235    }
236
237    pub(super) fn realtime_service_can_continue(&self) -> bool {
238        if self.imu_pending() {
239            return false;
240        }
241        let now_us = self.board.clock_micros();
242        !self.control_update_can_run_at(now_us) && self.realtime_service_has_control_slack(now_us)
243    }
244
245    pub(super) fn run_control_and_mixing_stage_if_control_due(&mut self, now_us: u64) -> bool {
246        if self.control_loop_rates.control_hz != 0 {
247            if !self.control_update_can_run_at(now_us) {
248                return false;
249            }
250            self.consume_control_update_deadline(now_us);
251            return self.run_control_and_mixing_stage_with_accumulated_imu();
252        }
253        if self.processed_sensors.imu.is_none() {
254            return false;
255        }
256        self.run_control_and_mixing_stage_if_new_imu()
257    }
258
259    pub(super) fn run_control_and_mixing_stage_with_accumulated_imu(&mut self) -> bool {
260        let Some(averaged_imu) = self.control_imu_accumulator.take_average() else {
261            return false;
262        };
263        let latest_imu = self.processed_sensors.imu;
264        self.processed_sensors.imu = Some(averaged_imu);
265        let ran_control = self.run_control_and_mixing_stage_if_new_imu();
266        self.processed_sensors.imu = latest_imu;
267        ran_control
268    }
269
270    pub fn run_control_and_mixing_stage_if_new_imu(&mut self) -> bool {
271        self.run_control_and_mixing_stage_if_new_imu_with_timing(None)
272    }
273
274    pub(super) fn run_control_and_mixing_stage_if_new_imu_with_timing(
275        &mut self,
276        timing: Option<&mut ControlPipelineTiming>,
277    ) -> bool {
278        run_control_pipeline_if_new_imu(ControlPipelineCtx {
279            board: &mut self.board,
280            params: &self.params,
281            sensors: &self.processed_sensors,
282            external_attitude: &mut self.external_attitude,
283            aux_commands: &self.aux_commands,
284            command: &self.command,
285            state: &mut self.state,
286            estimator: &mut self.estimator,
287            controller: &mut self.controller,
288            mixer: &mut self.mixer,
289            control_pipeline: &mut self.control_pipeline,
290            pwm_output: &self.pwm_output,
291            pwm: &mut self.pwm,
292            timing,
293        })
294    }
295    pub(super) fn update_board_leds(&mut self, now_ms: u32) {
296        if self.command.get_rc_override() != 0 {
297            self.board.led0_on();
298        } else {
299            self.board.led0_off();
300        }
301
302        if self.state.is_in_failsafe() {
303            if (now_ms / 100) % 2 == 0 {
304                self.board.led1_on();
305            } else {
306                self.board.led1_off();
307            }
308        } else if self.state.is_in_error_state() {
309            if (now_ms / 500) % 2 == 0 {
310                self.board.led1_on();
311            } else {
312                self.board.led1_off();
313            }
314        } else if self.state.is_armed() {
315            self.board.led1_on();
316        } else {
317            self.board.led1_off();
318        }
319    }
320}
veloxity_core/world/control.rs

veloxity_core/world/
control.rs
