veloxity_core/world/
telemetry.rs

1use super::*;
2
3impl<B, E, C, M, CI, PD, R> World<B, E, C, M, CI, PD, R>
4where
5    B: BoardIo,
6    E: Estimator<R>,
7    C: Controller<R, State = E::State> + RcTrimCalibrator,
8    M: crate::mixer::Mixer<R, MixerInput = C::ControlOutput>,
9    M::ActuatorCommands: AsRef<[R]> + Copy,
10    E::State: Copy + Default,
11    CI: CommInterface<B>,
12    PD: PwmDriver<R>,
13    R: FlightFloat,
14{
15    pub fn run_telemetry_stage(&mut self) {
16        let now_us = self.board.clock_micros();
17        if !self
18            .comm
19            .named_telemetry_due(now_us, &self.processed_sensors)
20        {
21            return;
22        }
23
24        let sensor_error_count = self.board.sensors_errors_count();
25        self.comm.send_named_telemetry_streams(TelemetryCtx {
26            board: &mut self.board,
27            now_us,
28            state: &self.state,
29            command: &self.command,
30            params: &self.params,
31            estimator_state: &self.control_pipeline.latest_estimator_state,
32            sensors: &self.processed_sensors,
33            actuator_commands: &self.control_pipeline.latest_pwm_outputs,
34            sensor_error_count,
35            loop_time_us: self.control_pipeline.latest_loop_time_us,
36        });
37    }
38
39    /// Sends up to `max_streams` currently due named telemetry streams.
40    ///
41    /// Board realtime loops may call this after a completed control update when hardware timing
42    /// shows enough post-control slack and service phases alone do not provide enough telemetry
43    /// scheduling opportunities. Keep the budget board-specific and validate the resulting control
44    /// p99/max timing with scope pins before making it a production default.
45    pub fn run_realtime_telemetry_stage_budgeted(&mut self, max_streams: usize) -> usize {
46        let mut sent = 0;
47        while sent < max_streams && self.send_realtime_telemetry_stream() {
48            sent += 1;
49        }
50        sent
51    }
52
53    /// Sends board-selected priority streams first, then fills the remaining budget normally.
54    ///
55    /// `priority_streams` is a board policy list. Core applies each stream's configured priority
56    /// gate, so boards can choose normal due-deadline behavior or a stricter freshness gate for
57    /// streams paced by the control tick.
58    pub fn run_realtime_telemetry_stage_prioritized(
59        &mut self,
60        priority_streams: &[RealtimeTelemetryPriority],
61        max_streams: usize,
62    ) -> usize {
63        let mut sent = 0;
64        for priority in priority_streams.iter().copied() {
65            if sent >= max_streams {
66                return sent;
67            }
68            if self.send_realtime_telemetry_stream_by_priority(priority) {
69                sent += 1;
70            }
71        }
72        while sent < max_streams && self.send_realtime_telemetry_stream() {
73            sent += 1;
74        }
75        sent
76    }
77
78    pub(super) fn send_realtime_telemetry_stream(&mut self) -> bool {
79        let now_us = self.board.clock_micros();
80        let sensor_error_count = self.board.sensors_errors_count();
81        self.comm.send_one_named_telemetry_stream(TelemetryCtx {
82            board: &mut self.board,
83            now_us,
84            state: &self.state,
85            command: &self.command,
86            params: &self.params,
87            estimator_state: &self.control_pipeline.latest_estimator_state,
88            sensors: &self.processed_sensors,
89            actuator_commands: &self.control_pipeline.latest_pwm_outputs,
90            sensor_error_count,
91            loop_time_us: self.control_pipeline.latest_loop_time_us,
92        })
93    }
94
95    pub(super) fn send_realtime_telemetry_stream_by_priority(
96        &mut self,
97        priority: RealtimeTelemetryPriority,
98    ) -> bool {
99        let now_us = self.board.clock_micros();
100        let sensor_error_count = self.board.sensors_errors_count();
101        self.comm.send_named_telemetry_stream_with_gate(
102            priority,
103            TelemetryCtx {
104                board: &mut self.board,
105                now_us,
106                state: &self.state,
107                command: &self.command,
108                params: &self.params,
109                estimator_state: &self.control_pipeline.latest_estimator_state,
110                sensors: &self.processed_sensors,
111                actuator_commands: &self.control_pipeline.latest_pwm_outputs,
112                sensor_error_count,
113                loop_time_us: self.control_pipeline.latest_loop_time_us,
114            },
115        )
116    }
117}
veloxity_core/world/telemetry.rs

veloxity_core/world/
telemetry.rs
