1use super::*;
2
3#[cfg(feature = "runtime-diagnostics")]
4use crate::comm::NamedTelemetryStream;
5#[cfg(feature = "runtime-diagnostics")]
6use core::sync::atomic::{AtomicU32, Ordering};
7
8#[cfg(feature = "runtime-diagnostics")]
9#[unsafe(no_mangle)]
10pub static VELOXITY_DIAG_IMU_UNSENT_OVERWRITE: AtomicU32 = AtomicU32::new(0);
11#[cfg(feature = "runtime-diagnostics")]
12#[unsafe(no_mangle)]
13pub static VELOXITY_DIAG_IMU_UNSENT_AGE_SUM_US: AtomicU32 = AtomicU32::new(0);
14#[cfg(feature = "runtime-diagnostics")]
15#[unsafe(no_mangle)]
16pub static VELOXITY_DIAG_IMU_UNSENT_AGE_MAX_US: AtomicU32 = AtomicU32::new(0);
17#[cfg(feature = "runtime-diagnostics")]
18#[unsafe(no_mangle)]
19pub static VELOXITY_DIAG_SERVICE_PHASE_COUNT: AtomicU32 = AtomicU32::new(0);
20#[cfg(feature = "runtime-diagnostics")]
21#[unsafe(no_mangle)]
22pub static VELOXITY_DIAG_SERVICE_PHASE_SUM_US: AtomicU32 = AtomicU32::new(0);
23#[cfg(feature = "runtime-diagnostics")]
24#[unsafe(no_mangle)]
25pub static VELOXITY_DIAG_SERVICE_PHASE_MAX_US: AtomicU32 = AtomicU32::new(0);
26#[cfg(feature = "runtime-diagnostics")]
27#[unsafe(no_mangle)]
28pub static VELOXITY_DIAG_ARMED_SERVICE_PHASE_COUNT: AtomicU32 = AtomicU32::new(0);
29#[cfg(feature = "runtime-diagnostics")]
30#[unsafe(no_mangle)]
31pub static VELOXITY_DIAG_ARMED_SERVICE_PHASE_SUM_US: AtomicU32 = AtomicU32::new(0);
32#[cfg(feature = "runtime-diagnostics")]
33#[unsafe(no_mangle)]
34pub static VELOXITY_DIAG_ARMED_SERVICE_PHASE_MAX_US: AtomicU32 = AtomicU32::new(0);
35
36#[cfg(feature = "runtime-diagnostics")]
37macro_rules! sensor_pipeline_counters {
38 ($input:ident, $output:ident, $unsent:ident) => {
39 #[unsafe(no_mangle)]
40 pub static $input: AtomicU32 = AtomicU32::new(0);
41 #[unsafe(no_mangle)]
42 pub static $output: AtomicU32 = AtomicU32::new(0);
43 #[unsafe(no_mangle)]
44 pub static $unsent: AtomicU32 = AtomicU32::new(0);
45 };
46}
47
48#[cfg(feature = "runtime-diagnostics")]
49sensor_pipeline_counters!(
50 VELOXITY_DIAG_MAG_PROCESS_INPUT,
51 VELOXITY_DIAG_MAG_PROCESS_OUTPUT,
52 VELOXITY_DIAG_MAG_UNSENT_OVERWRITE
53);
54#[cfg(feature = "runtime-diagnostics")]
55sensor_pipeline_counters!(
56 VELOXITY_DIAG_BARO_PROCESS_INPUT,
57 VELOXITY_DIAG_BARO_PROCESS_OUTPUT,
58 VELOXITY_DIAG_BARO_UNSENT_OVERWRITE
59);
60#[cfg(feature = "runtime-diagnostics")]
61sensor_pipeline_counters!(
62 VELOXITY_DIAG_PITOT_PROCESS_INPUT,
63 VELOXITY_DIAG_PITOT_PROCESS_OUTPUT,
64 VELOXITY_DIAG_PITOT_UNSENT_OVERWRITE
65);
66#[cfg(feature = "runtime-diagnostics")]
67sensor_pipeline_counters!(
68 VELOXITY_DIAG_RANGE_PROCESS_INPUT,
69 VELOXITY_DIAG_RANGE_PROCESS_OUTPUT,
70 VELOXITY_DIAG_RANGE_UNSENT_OVERWRITE
71);
72#[cfg(feature = "runtime-diagnostics")]
73sensor_pipeline_counters!(
74 VELOXITY_DIAG_GNSS_PROCESS_INPUT,
75 VELOXITY_DIAG_GNSS_PROCESS_OUTPUT,
76 VELOXITY_DIAG_GNSS_UNSENT_OVERWRITE
77);
78#[cfg(feature = "runtime-diagnostics")]
79sensor_pipeline_counters!(
80 VELOXITY_DIAG_BATTERY_PROCESS_INPUT,
81 VELOXITY_DIAG_BATTERY_PROCESS_OUTPUT,
82 VELOXITY_DIAG_BATTERY_UNSENT_OVERWRITE
83);
84#[cfg(feature = "runtime-diagnostics")]
85sensor_pipeline_counters!(
86 VELOXITY_DIAG_RC_PROCESS_INPUT,
87 VELOXITY_DIAG_RC_PROCESS_OUTPUT,
88 VELOXITY_DIAG_RC_UNSENT_OVERWRITE
89);
90#[cfg(feature = "runtime-diagnostics")]
91#[unsafe(no_mangle)]
92pub static VELOXITY_DIAG_IMU_PROCESS_INPUT: AtomicU32 = AtomicU32::new(0);
93#[cfg(feature = "runtime-diagnostics")]
94#[unsafe(no_mangle)]
95pub static VELOXITY_DIAG_IMU_PROCESS_OUTPUT: AtomicU32 = AtomicU32::new(0);
96
97impl<B, E, C, M, CI, PD, R> World<B, E, C, M, CI, PD, R>
98where
99 B: BoardIo,
100 E: Estimator<R>,
101 C: Controller<R, State = E::State> + RcTrimCalibrator,
102 M: crate::mixer::Mixer<R, MixerInput = C::ControlOutput>,
103 M::ActuatorCommands: AsRef<[R]> + Copy,
104 E::State: Copy + Default,
105 CI: CommInterface<B>,
106 PD: PwmDriver<R>,
107 R: FlightFloat,
108{
109 pub fn run_prioritized_service_steps_with_policy(
110 &mut self,
111 policy: RealtimeServicePolicy,
112 ) -> WorldReport {
113 let pass_start_us = self.board.clock_micros();
114 let mut result = WorldReport {
115 had_rx: self.board.serial_rx_pending(),
116 ..WorldReport::default()
117 };
118
119 while self.realtime_service_can_continue() {
120 let step_result = self.run_prioritized_service_step(policy);
121 let had_service_activity = step_result.had_rx
122 || step_result.had_raw_sensor
123 || step_result.telemetry_due
124 || step_result.telemetry_deferred;
125 result.merge_from(step_result);
126
127 if policy.min_spacing_us != 0 {
128 break;
129 }
130 if !had_service_activity && !policy.continue_when_idle {
131 break;
132 }
133 }
134
135 self.next_realtime_service_us = self
136 .board
137 .clock_micros()
138 .saturating_add(policy.min_spacing_us);
139 result.elapsed_after_control_us = self
140 .board
141 .clock_micros()
142 .saturating_sub(pass_start_us)
143 .min(u32::MAX as u64) as u32;
144 #[cfg(feature = "runtime-diagnostics")]
145 {
146 VELOXITY_DIAG_SERVICE_PHASE_COUNT.fetch_add(1, Ordering::Relaxed);
147 VELOXITY_DIAG_SERVICE_PHASE_SUM_US
148 .fetch_add(result.elapsed_after_control_us, Ordering::Relaxed);
149 VELOXITY_DIAG_SERVICE_PHASE_MAX_US
150 .fetch_max(result.elapsed_after_control_us, Ordering::Relaxed);
151 if self.state.is_armed() {
152 VELOXITY_DIAG_ARMED_SERVICE_PHASE_COUNT.fetch_add(1, Ordering::Relaxed);
153 VELOXITY_DIAG_ARMED_SERVICE_PHASE_SUM_US
154 .fetch_add(result.elapsed_after_control_us, Ordering::Relaxed);
155 VELOXITY_DIAG_ARMED_SERVICE_PHASE_MAX_US
156 .fetch_max(result.elapsed_after_control_us, Ordering::Relaxed);
157 }
158 }
159 result
160 }
161
162 pub(super) fn run_prioritized_service_step(
163 &mut self,
164 policy: RealtimeServicePolicy,
165 ) -> WorldReport {
166 let mut result = WorldReport::default();
167
168 if self.realtime_service_can_continue() {
174 result.telemetry_due |= if policy.drain_telemetry_with_available_slack {
175 self.run_realtime_telemetry_stage_with_available_slack() != 0
176 } else {
177 self.run_realtime_telemetry_stage_budgeted(policy.telemetry_streams_per_phase) != 0
178 };
179 }
180
181 let sensor_result = if self.realtime_service_can_continue() {
182 self.run_service_sensor_stage()
183 } else {
184 WorldReport::default()
185 };
186 result.merge_from(sensor_result);
187
188 if self.realtime_service_can_continue() {
189 result.had_rx |= self.board.serial_rx_pending();
190 self.run_service_input_stage();
191 }
192
193 if self.realtime_service_can_continue() {
194 let fresh_rc = if sensor_result.had_raw_rc {
195 self.processed_sensors.rc
196 } else {
197 None
198 };
199 self.run_rc_command_state_stages(fresh_rc);
200 result.had_processed_rc =
201 sensor_result.had_raw_rc && self.processed_sensors.rc.is_some();
202 }
203
204 if self.realtime_service_can_continue() {
205 self.drain_logs_and_send_responses_limited(REALTIME_SERVICE_RESPONSE_BUDGET);
206 }
207
208 if self.realtime_service_can_continue() {
209 self.board.serial_flush_budgeted(1);
210 }
211
212 if self.realtime_service_can_continue() {
213 self.board.run_deferred_board_actions();
214 }
215
216 result
217 }
218
219 pub(super) fn run_service_input_stage(&mut self) {
220 self.run_communication_and_parameter_service_stage();
221 }
222
223 pub(super) fn run_service_sensor_stage(&mut self) -> WorldReport {
224 let now_us = self.board.clock_micros();
225 let latest_imu = self.processed_sensors.imu;
226 let latest_mag = self.processed_sensors.mag;
227 let latest_baro = self.processed_sensors.baro;
228 let latest_pitot = self.processed_sensors.pitot;
229 let latest_range = self.processed_sensors.range;
230 let latest_gnss = self.processed_sensors.gnss;
231 let latest_battery = self.processed_sensors.battery;
232 let latest_rc = self.processed_sensors.rc;
233 let latest_attitude = self.processed_sensors.attitude;
234
235 self.board.update_service_sensor_bus(&mut self.raw_sensors);
236 let had_raw_imu = self.raw_sensors.imu.is_some();
237 let had_raw_mag = self.raw_sensors.mag.is_some();
238 let had_raw_baro = self.raw_sensors.baro.is_some();
239 let had_raw_pitot = self.raw_sensors.pitot.is_some();
240 let had_raw_range = self.raw_sensors.range.is_some();
241 let had_raw_gnss = self.raw_sensors.gnss.is_some();
242 let had_raw_battery = self.raw_sensors.battery.is_some();
243 let had_raw_rc = self.raw_sensors.rc.is_some();
244 let had_raw_attitude = self.raw_sensors.attitude.is_some();
245 self.process_sensor_bus_after_update();
246
247 #[cfg(feature = "runtime-diagnostics")]
248 {
249 macro_rules! record_pipeline {
250 ($had_raw:ident, $field:ident, $input:ident, $output:ident) => {
251 if $had_raw {
252 $input.fetch_add(1, Ordering::Relaxed);
253 if self.processed_sensors.$field.is_some() {
254 $output.fetch_add(1, Ordering::Relaxed);
255 }
256 }
257 };
258 }
259 record_pipeline!(
260 had_raw_mag,
261 mag,
262 VELOXITY_DIAG_MAG_PROCESS_INPUT,
263 VELOXITY_DIAG_MAG_PROCESS_OUTPUT
264 );
265 record_pipeline!(
266 had_raw_baro,
267 baro,
268 VELOXITY_DIAG_BARO_PROCESS_INPUT,
269 VELOXITY_DIAG_BARO_PROCESS_OUTPUT
270 );
271 record_pipeline!(
272 had_raw_pitot,
273 pitot,
274 VELOXITY_DIAG_PITOT_PROCESS_INPUT,
275 VELOXITY_DIAG_PITOT_PROCESS_OUTPUT
276 );
277 record_pipeline!(
278 had_raw_range,
279 range,
280 VELOXITY_DIAG_RANGE_PROCESS_INPUT,
281 VELOXITY_DIAG_RANGE_PROCESS_OUTPUT
282 );
283 record_pipeline!(
284 had_raw_gnss,
285 gnss,
286 VELOXITY_DIAG_GNSS_PROCESS_INPUT,
287 VELOXITY_DIAG_GNSS_PROCESS_OUTPUT
288 );
289 record_pipeline!(
290 had_raw_battery,
291 battery,
292 VELOXITY_DIAG_BATTERY_PROCESS_INPUT,
293 VELOXITY_DIAG_BATTERY_PROCESS_OUTPUT
294 );
295 record_pipeline!(
296 had_raw_rc,
297 rc,
298 VELOXITY_DIAG_RC_PROCESS_INPUT,
299 VELOXITY_DIAG_RC_PROCESS_OUTPUT
300 );
301
302 macro_rules! record_unsent {
303 ($had_raw:ident, $before:ident, $field:ident, $stream:ident, $counter:ident) => {
304 if $had_raw
305 && let (Some(previous), Some(current)) =
306 ($before, self.processed_sensors.$field)
307 && previous.header.timestamp != current.header.timestamp
308 && !self.comm.telemetry_sample_was_sent(
309 NamedTelemetryStream::$stream,
310 previous.header.timestamp,
311 )
312 {
313 $counter.fetch_add(1, Ordering::Relaxed);
314 }
315 };
316 }
317 record_unsent!(
318 had_raw_mag,
319 latest_mag,
320 mag,
321 Mag,
322 VELOXITY_DIAG_MAG_UNSENT_OVERWRITE
323 );
324 record_unsent!(
325 had_raw_baro,
326 latest_baro,
327 baro,
328 Baro,
329 VELOXITY_DIAG_BARO_UNSENT_OVERWRITE
330 );
331 record_unsent!(
332 had_raw_pitot,
333 latest_pitot,
334 pitot,
335 DiffPressure,
336 VELOXITY_DIAG_PITOT_UNSENT_OVERWRITE
337 );
338 record_unsent!(
339 had_raw_range,
340 latest_range,
341 range,
342 Range,
343 VELOXITY_DIAG_RANGE_UNSENT_OVERWRITE
344 );
345 record_unsent!(
346 had_raw_gnss,
347 latest_gnss,
348 gnss,
349 Gnss,
350 VELOXITY_DIAG_GNSS_UNSENT_OVERWRITE
351 );
352 record_unsent!(
353 had_raw_battery,
354 latest_battery,
355 battery,
356 Battery,
357 VELOXITY_DIAG_BATTERY_UNSENT_OVERWRITE
358 );
359 record_unsent!(
360 had_raw_rc,
361 latest_rc,
362 rc,
363 Rc,
364 VELOXITY_DIAG_RC_UNSENT_OVERWRITE
365 );
366 }
367
368 if !had_raw_imu {
369 self.processed_sensors.imu = latest_imu;
370 }
371 if !had_raw_mag {
372 self.processed_sensors.mag = latest_mag;
373 }
374 if !had_raw_baro {
375 self.processed_sensors.baro = latest_baro;
376 }
377 if !had_raw_pitot {
378 self.processed_sensors.pitot = latest_pitot;
379 }
380 if !had_raw_range {
381 self.processed_sensors.range = latest_range;
382 }
383 if !had_raw_gnss {
384 self.processed_sensors.gnss = latest_gnss;
385 }
386 if !had_raw_battery {
387 self.processed_sensors.battery = latest_battery;
388 }
389 if !had_raw_rc {
390 self.processed_sensors.rc = latest_rc;
391 }
392 if !had_raw_attitude {
393 self.processed_sensors.attitude = latest_attitude;
394 }
395
396 self.update_sensor_health_and_calibration(now_us);
397 WorldReport {
398 had_raw_sensor: had_raw_imu
399 || had_raw_mag
400 || had_raw_baro
401 || had_raw_pitot
402 || had_raw_range
403 || had_raw_gnss
404 || had_raw_battery
405 || had_raw_rc
406 || had_raw_attitude,
407 had_raw_imu,
408 had_raw_baro,
409 had_raw_rc,
410 had_processed_imu: self.processed_sensors.imu.is_some(),
411 had_processed_baro: self.processed_sensors.baro.is_some(),
412 had_processed_rc: self.processed_sensors.rc.is_some(),
413 ..WorldReport::default()
414 }
415 }
416
417 pub fn run_communication_and_parameter_service_stage(&mut self) {
418 self.process_comm_stage();
419 if self.has_pending_companion_work() {
420 self.apply_companion_events();
421 }
422 if !self.command_events.is_empty() {
423 self.apply_command_events();
424 }
425 if self.has_pending_param_work() {
426 self.service_param_events();
427 }
428 self.request_gyro_calibration_if_needed();
429 if self.param_events.full_refresh || !self.param_events.changes.is_empty() {
430 self.apply_param_reactions();
431 }
432 }
433
434 pub fn run_sensor_ingestion_and_health_stage(&mut self) {
435 self.run_sensor_ingestion_and_health_stage_without_log_drain();
436 self.drain_logs_and_send_responses();
437 }
438
439 pub(super) fn run_sensor_ingestion_and_health_stage_without_log_drain(&mut self) {
440 let now_us = self.board.clock_micros();
441
442 self.run_sensor_ingestion_stage();
443 self.update_sensor_health_and_calibration(now_us);
444 }
445
446 pub(super) fn process_comm_stage(&mut self) {
447 self.comm.process_incoming_messages(&mut self.board);
448 if !self.comm.has_pending_messages() {
449 return;
450 }
451 self.comm.act_on_messages(
452 &mut self.param_events,
453 &mut self.comm_events,
454 &mut self.command_events,
455 &mut self.companion_events,
456 &mut self.board,
457 );
458 }
459
460 pub(super) fn has_pending_companion_work(&self) -> bool {
461 !self.companion_events.is_empty()
462 || (self.companion_link.connected && self.pending_hard_error.is_some())
463 }
464
465 pub(super) fn has_pending_param_work(&self) -> bool {
466 !self.param_events.set_requests.is_empty()
467 || !self.param_events.read_requests.is_empty()
468 || !self.param_events.list_requests.is_empty()
469 || self.param_list_state.is_active()
470 }
471
472 pub(super) fn apply_companion_events(&mut self) {
473 companion::apply_companion_inputs(&mut CompanionInputCtx {
474 events: &mut self.companion_events,
475 comm_events: &mut self.comm_events,
476 link: &mut self.companion_link,
477 aux_commands: &mut self.aux_commands,
478 external_attitude: &mut self.external_attitude,
479 pending_hard_error: &mut self.pending_hard_error,
480 });
481 }
482
483 pub(super) fn apply_command_events(&mut self) {
484 command_service::apply_command_requests(&mut CommandRequestCtx {
485 requests: &mut self.command_events,
486 param_events: &mut self.param_events,
487 comm_events: &mut self.comm_events,
488 state: &self.state,
489 command: &mut self.command,
490 controller: &mut self.controller,
491 board: &mut self.board,
492 flags: &mut self.cal_flags,
493 params: &mut self.params,
494 });
495 }
496
497 pub(super) fn service_param_events(&mut self) {
498 param_service::service_param_events(&mut ParamServiceCtx {
499 params: &mut self.params,
500 state: &mut self.param_list_state,
501 events: &mut self.param_events,
502 comm_events: &mut self.comm_events,
503 });
504 }
505
506 pub(super) fn apply_param_reactions(&mut self) {
507 let battery_monitor_changed = self.param_events.full_refresh
508 || self.param_events.changes.iter().any(|change| {
509 matches!(
510 change.id,
511 ParamId::PARAM_BATTERY_VOLTAGE_MULTIPLIER
512 | ParamId::PARAM_BATTERY_CURRENT_MULTIPLIER
513 )
514 });
515 if battery_monitor_changed {
516 let voltage_multiplier = match self
517 .params
518 .get_by_id(ParamId::PARAM_BATTERY_VOLTAGE_MULTIPLIER)
519 {
520 ParamValue::Float(value) => value,
521 _ => 0.0,
522 };
523 let current_multiplier = match self
524 .params
525 .get_by_id(ParamId::PARAM_BATTERY_CURRENT_MULTIPLIER)
526 {
527 ParamValue::Float(value) => value,
528 _ => 0.0,
529 };
530 self.board
531 .configure_battery_monitor(voltage_multiplier, current_multiplier);
532 }
533 if self.param_events.full_refresh {
534 self.comm.configure_telemetry_from_params(&self.params);
535 } else {
536 let now_us = self.board.clock_micros();
537 for change in self.param_events.changes.iter() {
538 self.comm
539 .update_telemetry_param(&self.params, change.id, now_us);
540 }
541 }
542 reactions::apply_param_reactions(&mut ParamReactionCtx {
543 events: &mut self.param_events,
544 params: &self.params,
545 rc: &mut self.rc,
546 command: &mut self.command,
547 state: &mut self.state,
548 estimator: &mut self.estimator,
549 controller: &mut self.controller,
550 mixer: &mut self.mixer,
551 control_pipeline: &mut self.control_pipeline,
552 });
553 }
554
555 pub(super) fn request_gyro_calibration_if_needed(&mut self) {
556 if self.state.is_calibrating() && !self.cal_flags.contains(CalibrationFlags::GYRO) {
557 self.cal_flags.remove(CalibrationFlags::GYRO_FAILED);
558 param_service::set_param_and_emit_change(
559 &mut self.params,
560 &mut self.param_events.changes,
561 ParamId::PARAM_GYRO_X_BIAS,
562 ParamValue::Float(0.0),
563 );
564 param_service::set_param_and_emit_change(
565 &mut self.params,
566 &mut self.param_events.changes,
567 ParamId::PARAM_GYRO_Y_BIAS,
568 ParamValue::Float(0.0),
569 );
570 param_service::set_param_and_emit_change(
571 &mut self.params,
572 &mut self.param_events.changes,
573 ParamId::PARAM_GYRO_Z_BIAS,
574 ParamValue::Float(0.0),
575 );
576 self.cal_flags.insert(CalibrationFlags::GYRO);
577 }
578 }
579
580 pub(super) fn process_sensor_bus_after_update(&mut self) {
581 let now_ms = self.board.clock_millis();
582 let calibration_flags_before = self.cal_flags;
583 let baro_bias_before = self.params.get_by_id(ParamId::PARAM_BARO_BIAS);
584 let ground_level_before = self.params.get_by_id(ParamId::PARAM_GROUND_LEVEL);
585 process_sensor_bus(SensorIngestionCtx {
586 raw: &mut self.raw_sensors,
587 processed: &mut self.processed_sensors,
588 processors: &mut self.sensor_processors,
589 flags: &mut self.cal_flags,
590 params: &mut self.params,
591 now_ms,
592 });
593 if calibration_flags_before.contains(CalibrationFlags::BARO)
594 && !self.cal_flags.contains(CalibrationFlags::BARO)
595 && !self.cal_flags.contains(CalibrationFlags::BARO_FAILED)
596 {
597 param_service::emit_param_change(
602 &mut self.param_events.changes,
603 ParamId::PARAM_BARO_BIAS,
604 baro_bias_before,
605 self.params.get_by_id(ParamId::PARAM_BARO_BIAS),
606 );
607 param_service::emit_param_change(
608 &mut self.param_events.changes,
609 ParamId::PARAM_GROUND_LEVEL,
610 ground_level_before,
611 self.params.get_by_id(ParamId::PARAM_GROUND_LEVEL),
612 );
613 }
614 if calibration_flags_before.contains(CalibrationFlags::GYRO)
615 && !self.cal_flags.contains(CalibrationFlags::GYRO)
616 && !self.cal_flags.contains(CalibrationFlags::GYRO_FAILED)
617 {
618 self.estimator.reset_adaptive_bias();
619 }
620 if calibration_flags_before.contains(CalibrationFlags::ACCEL)
621 && !self.cal_flags.contains(CalibrationFlags::ACCEL)
622 && !self.cal_flags.contains(CalibrationFlags::ACCEL_FAILED)
623 {
624 self.state
628 .set_error_flag(ErrorFlag::UNCALIBRATED_IMU, false, &self.params);
629 self.estimator.reset();
630 self.control_pipeline = ControlPipelineResource::default();
631 }
632 }
633
634 pub(super) fn process_imu_sensor_after_update(&mut self) {
635 #[cfg(feature = "runtime-diagnostics")]
636 let previous_imu = self.processed_sensors.imu;
637 #[cfg(feature = "runtime-diagnostics")]
638 let had_raw_imu = self.raw_sensors.imu.is_some();
639 let now_ms = self.board.clock_millis();
640 let calibration_flags_before = self.cal_flags;
641 process_imu_sensor(SensorIngestionCtx {
642 raw: &mut self.raw_sensors,
643 processed: &mut self.processed_sensors,
644 processors: &mut self.sensor_processors,
645 flags: &mut self.cal_flags,
646 params: &mut self.params,
647 now_ms,
648 });
649 #[cfg(feature = "runtime-diagnostics")]
650 if had_raw_imu {
651 VELOXITY_DIAG_IMU_PROCESS_INPUT.fetch_add(1, Ordering::Relaxed);
652 if self.processed_sensors.imu.is_some() {
653 VELOXITY_DIAG_IMU_PROCESS_OUTPUT.fetch_add(1, Ordering::Relaxed);
654 }
655 }
656 #[cfg(feature = "runtime-diagnostics")]
657 if let (Some(previous), Some(current)) = (previous_imu, self.processed_sensors.imu)
658 && previous.header.timestamp != current.header.timestamp
659 && !self
660 .comm
661 .telemetry_sample_was_sent(NamedTelemetryStream::Imu, previous.header.timestamp)
662 {
663 VELOXITY_DIAG_IMU_UNSENT_OVERWRITE.fetch_add(1, Ordering::Relaxed);
664 let age_us = self
665 .board
666 .clock_micros()
667 .saturating_sub(previous.header.timestamp)
668 .min(u32::MAX as u64) as u32;
669 VELOXITY_DIAG_IMU_UNSENT_AGE_SUM_US.fetch_add(age_us, Ordering::Relaxed);
670 VELOXITY_DIAG_IMU_UNSENT_AGE_MAX_US.fetch_max(age_us, Ordering::Relaxed);
671 }
672 if calibration_flags_before.contains(CalibrationFlags::GYRO)
673 && !self.cal_flags.contains(CalibrationFlags::GYRO)
674 && !self.cal_flags.contains(CalibrationFlags::GYRO_FAILED)
675 {
676 self.estimator.reset_adaptive_bias();
677 }
678 if calibration_flags_before.contains(CalibrationFlags::ACCEL)
679 && !self.cal_flags.contains(CalibrationFlags::ACCEL)
680 && !self.cal_flags.contains(CalibrationFlags::ACCEL_FAILED)
681 {
682 self.state
685 .set_error_flag(ErrorFlag::UNCALIBRATED_IMU, false, &self.params);
686 self.estimator.reset();
687 self.control_pipeline = ControlPipelineResource::default();
688 }
689 }
690
691 pub(super) fn record_control_imu_candidate(&mut self) {
692 if let Some(imu) = self.processed_sensors.imu {
693 self.control_imu_accumulator.push(imu);
694 }
695 }
696
697 pub(super) fn run_sensor_ingestion_stage(&mut self) {
698 self.board.update_sensor_bus(&mut self.raw_sensors);
699 self.process_sensor_bus_after_update();
700 }
701
702 pub(super) fn drain_logs_and_send_responses(&mut self) {
703 log_drain::drain_logs_to_comm_responses(LogDrainCtx {
704 responses: EventEmitPort::new(&mut self.comm_events.responses),
705 connected: self.companion_link.connected,
706 });
707 if self.comm_events.is_empty() {
708 return;
709 }
710 self.comm
711 .send_comm_responses(&mut self.board, &mut self.comm_events);
712 }
713
714 pub(super) fn drain_logs_and_send_responses_limited(&mut self, max_responses: usize) {
715 log_drain::drain_logs_to_comm_responses(LogDrainCtx {
716 responses: EventEmitPort::new(&mut self.comm_events.responses),
717 connected: self.companion_link.connected,
718 });
719 if self.comm_events.is_empty() {
720 return;
721 }
722 self.comm.send_comm_responses_limited(
723 &mut self.board,
724 &mut self.comm_events,
725 max_responses,
726 );
727 }
728
729 pub(super) fn update_sensor_health_and_calibration(&mut self, now_us: u64) {
730 update_sensor_health(SensorHealthCtx {
731 now_us,
732 sensors: &self.processed_sensors,
733 params: &self.params,
734 state: &mut self.state,
735 last_imu_seen: &mut self.last_imu_seen,
736 imu_timeout_us: IMU_TIMEOUT_US,
737 });
738
739 let failed_arm_calibration =
740 self.state.is_calibrating() && self.cal_flags.contains(CalibrationFlags::GYRO_FAILED);
741 let completed_arm_calibration =
742 self.state.is_calibrating() && !self.cal_flags.contains(CalibrationFlags::GYRO);
743
744 if failed_arm_calibration {
745 self.state.update(Event::CALIBRATION_FAILED, &self.params);
746 self.cal_flags.remove(CalibrationFlags::GYRO_FAILED);
747 } else if completed_arm_calibration {
748 self.state.update(Event::CALIBRATION_COMPLETE, &self.params);
749 }
750 }
751}