veloxity_core/world/
telemetry.rs

1use super::*;
2
3#[cfg(feature = "runtime-diagnostics")]
4use core::sync::atomic::{AtomicU32, Ordering};
5
6#[cfg(feature = "runtime-diagnostics")]
7#[unsafe(no_mangle)]
8pub static VELOXITY_DIAG_TELEMETRY_DRAIN_COUNT: AtomicU32 = AtomicU32::new(0);
9#[cfg(feature = "runtime-diagnostics")]
10#[unsafe(no_mangle)]
11pub static VELOXITY_DIAG_TELEMETRY_DRAIN_MESSAGES: AtomicU32 = AtomicU32::new(0);
12#[cfg(feature = "runtime-diagnostics")]
13#[unsafe(no_mangle)]
14pub static VELOXITY_DIAG_TELEMETRY_DRAIN_SUM_US: AtomicU32 = AtomicU32::new(0);
15#[cfg(feature = "runtime-diagnostics")]
16#[unsafe(no_mangle)]
17pub static VELOXITY_DIAG_TELEMETRY_DRAIN_MAX_US: AtomicU32 = AtomicU32::new(0);
18
19impl<B, E, C, M, CI, PD, R> World<B, E, C, M, CI, PD, R>
20where
21    B: BoardIo,
22    E: Estimator<R>,
23    C: Controller<R, State = E::State> + RcTrimCalibrator,
24    M: crate::mixer::Mixer<R, MixerInput = C::ControlOutput>,
25    M::ActuatorCommands: AsRef<[R]> + Copy,
26    E::State: Copy + Default,
27    CI: CommInterface<B>,
28    PD: PwmDriver<R>,
29    R: FlightFloat,
30{
31    pub fn run_telemetry_stage(&mut self) {
32        let now_us = self.board.clock_micros();
33        if !self
34            .comm
35            .named_telemetry_due(now_us, &self.processed_sensors)
36        {
37            return;
38        }
39
40        let sensor_error_count = self.board.sensors_errors_count();
41        self.comm.send_named_telemetry_streams(TelemetryCtx {
42            board: &mut self.board,
43            now_us,
44            state: &self.state,
45            command: &self.command,
46            params: &self.params,
47            estimator_state: &self.control_pipeline.latest_estimator_state,
48            sensors: &self.processed_sensors,
49            actuator_commands: &self.control_pipeline.latest_pwm_outputs,
50            sensor_error_count,
51            loop_time_us: self.control_pipeline.latest_loop_time_us,
52        });
53    }
54
55    /// Sends up to `max_streams` currently due named telemetry streams.
56    ///
57    /// Board realtime loops may call this after a completed control update when hardware timing
58    /// shows enough post-control slack and service phases alone do not provide enough telemetry
59    /// scheduling opportunities. Keep the budget board-specific and validate the resulting control
60    /// p99/max timing with scope pins before making it a production default.
61    pub fn run_realtime_telemetry_stage_budgeted(&mut self, max_streams: usize) -> usize {
62        let mut sent = 0;
63        while sent < max_streams && self.send_realtime_telemetry_stream() {
64            sent += 1;
65        }
66        sent
67    }
68
69    /// Sends due streams until there is no work left or the measured control
70    /// slack reaches the board's safety margin. The slack check occurs between
71    /// individual streams, so this never turns an arbitrary stream count into
72    /// a scheduling bottleneck.
73    pub fn run_realtime_telemetry_stage_with_available_slack(&mut self) -> usize {
74        #[cfg(feature = "runtime-diagnostics")]
75        let started_us = self.board.clock_micros();
76        let mut sent = 0;
77        while self.realtime_service_can_continue() && self.send_realtime_telemetry_stream() {
78            sent += 1;
79        }
80        #[cfg(feature = "runtime-diagnostics")]
81        if sent != 0 {
82            let elapsed_us = self
83                .board
84                .clock_micros()
85                .saturating_sub(started_us)
86                .min(u32::MAX as u64) as u32;
87            VELOXITY_DIAG_TELEMETRY_DRAIN_COUNT.fetch_add(1, Ordering::Relaxed);
88            VELOXITY_DIAG_TELEMETRY_DRAIN_MESSAGES.fetch_add(sent as u32, Ordering::Relaxed);
89            VELOXITY_DIAG_TELEMETRY_DRAIN_SUM_US.fetch_add(elapsed_us, Ordering::Relaxed);
90            VELOXITY_DIAG_TELEMETRY_DRAIN_MAX_US.fetch_max(elapsed_us, Ordering::Relaxed);
91        }
92        sent
93    }
94
95    /// Sends board-selected priority streams first, then fills the remaining budget normally.
96    ///
97    /// `priority_streams` is a board policy list. Core applies each stream's configured priority
98    /// gate, so boards can choose normal due-deadline behavior or a stricter freshness gate for
99    /// streams paced by the control tick.
100    pub fn run_realtime_telemetry_stage_prioritized(
101        &mut self,
102        priority_streams: &[RealtimeTelemetryPriority],
103        max_streams: usize,
104    ) -> usize {
105        let mut sent = 0;
106        for priority in priority_streams.iter().copied() {
107            if sent >= max_streams {
108                return sent;
109            }
110            if self.send_realtime_telemetry_stream_by_priority(priority) {
111                sent += 1;
112            }
113        }
114        while sent < max_streams && self.send_realtime_telemetry_stream() {
115            sent += 1;
116        }
117        sent
118    }
119
120    pub(super) fn send_realtime_telemetry_stream(&mut self) -> bool {
121        let now_us = self.board.clock_micros();
122        let sensor_error_count = self.board.sensors_errors_count();
123        self.comm.send_one_named_telemetry_stream(TelemetryCtx {
124            board: &mut self.board,
125            now_us,
126            state: &self.state,
127            command: &self.command,
128            params: &self.params,
129            estimator_state: &self.control_pipeline.latest_estimator_state,
130            sensors: &self.processed_sensors,
131            actuator_commands: &self.control_pipeline.latest_pwm_outputs,
132            sensor_error_count,
133            loop_time_us: self.control_pipeline.latest_loop_time_us,
134        })
135    }
136
137    pub(super) fn send_realtime_telemetry_stream_by_priority(
138        &mut self,
139        priority: RealtimeTelemetryPriority,
140    ) -> bool {
141        let now_us = self.board.clock_micros();
142        let sensor_error_count = self.board.sensors_errors_count();
143        self.comm.send_named_telemetry_stream_with_gate(
144            priority,
145            TelemetryCtx {
146                board: &mut self.board,
147                now_us,
148                state: &self.state,
149                command: &self.command,
150                params: &self.params,
151                estimator_state: &self.control_pipeline.latest_estimator_state,
152                sensors: &self.processed_sensors,
153                actuator_commands: &self.control_pipeline.latest_pwm_outputs,
154                sensor_error_count,
155                loop_time_us: self.control_pipeline.latest_loop_time_us,
156            },
157        )
158    }
159}
veloxity_core/world/telemetry.rs

veloxity_core/world/
telemetry.rs
