veloxity_core/world/
service.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_prioritized_service_steps_with_policy(
16        &mut self,
17        policy: RealtimeServicePolicy,
18    ) -> WorldReport {
19        let pass_start_us = self.board.clock_micros();
20        let mut result = WorldReport {
21            had_rx: self.board.serial_rx_pending(),
22            ..WorldReport::default()
23        };
24
25        while self.realtime_service_can_continue() {
26            let step_result = self.run_prioritized_service_step(policy);
27            let had_service_activity = step_result.had_rx
28                || step_result.had_raw_sensor
29                || step_result.telemetry_due
30                || step_result.telemetry_deferred;
31            result.merge_from(step_result);
32
33            if policy.min_spacing_us != 0 {
34                break;
35            }
36            if !had_service_activity && !policy.continue_when_idle {
37                break;
38            }
39        }
40
41        self.next_realtime_service_us = self
42            .board
43            .clock_micros()
44            .saturating_add(policy.min_spacing_us);
45        result.elapsed_after_control_us = self
46            .board
47            .clock_micros()
48            .saturating_sub(pass_start_us)
49            .min(u32::MAX as u64) as u32;
50        result
51    }
52
53    pub(super) fn run_prioritized_service_step(
54        &mut self,
55        policy: RealtimeServicePolicy,
56    ) -> WorldReport {
57        let mut result = WorldReport::default();
58
59        let sensor_result = self.run_service_sensor_stage();
60        result.merge_from(sensor_result);
61
62        if self.realtime_service_can_continue() {
63            result.had_rx |= self.board.serial_rx_pending();
64            self.run_service_input_stage();
65        }
66
67        if self.realtime_service_can_continue() {
68            let fresh_rc = if sensor_result.had_raw_rc {
69                self.processed_sensors.rc
70            } else {
71                None
72            };
73            self.run_rc_command_state_stages(fresh_rc);
74            result.had_processed_rc =
75                sensor_result.had_raw_rc && self.processed_sensors.rc.is_some();
76        }
77
78        if self.realtime_service_can_continue() {
79            self.drain_logs_and_send_responses_limited(REALTIME_SERVICE_RESPONSE_BUDGET);
80        }
81
82        if self.realtime_service_can_continue() {
83            result.telemetry_due |=
84                self.run_realtime_telemetry_stage_budgeted(policy.telemetry_streams_per_phase) != 0;
85        }
86
87        if self.realtime_service_can_continue() {
88            self.board.serial_flush_budgeted(1);
89        }
90
91        if self.realtime_service_can_continue() {
92            self.board.run_deferred_board_actions();
93        }
94
95        result
96    }
97
98    pub(super) fn run_service_input_stage(&mut self) {
99        self.run_communication_and_parameter_service_stage();
100    }
101
102    pub(super) fn run_service_sensor_stage(&mut self) -> WorldReport {
103        let now_us = self.board.clock_micros();
104        let latest_imu = self.processed_sensors.imu;
105        let latest_mag = self.processed_sensors.mag;
106        let latest_baro = self.processed_sensors.baro;
107        let latest_pitot = self.processed_sensors.pitot;
108        let latest_range = self.processed_sensors.range;
109        let latest_gnss = self.processed_sensors.gnss;
110        let latest_battery = self.processed_sensors.battery;
111        let latest_rc = self.processed_sensors.rc;
112        let latest_attitude = self.processed_sensors.attitude;
113
114        self.board.update_service_sensor_bus(&mut self.raw_sensors);
115        let had_raw_imu = self.raw_sensors.imu.is_some();
116        let had_raw_mag = self.raw_sensors.mag.is_some();
117        let had_raw_baro = self.raw_sensors.baro.is_some();
118        let had_raw_pitot = self.raw_sensors.pitot.is_some();
119        let had_raw_range = self.raw_sensors.range.is_some();
120        let had_raw_gnss = self.raw_sensors.gnss.is_some();
121        let had_raw_battery = self.raw_sensors.battery.is_some();
122        let had_raw_rc = self.raw_sensors.rc.is_some();
123        let had_raw_attitude = self.raw_sensors.attitude.is_some();
124        self.process_sensor_bus_after_update();
125
126        if !had_raw_imu {
127            self.processed_sensors.imu = latest_imu;
128        }
129        if !had_raw_mag {
130            self.processed_sensors.mag = latest_mag;
131        }
132        if !had_raw_baro {
133            self.processed_sensors.baro = latest_baro;
134        }
135        if !had_raw_pitot {
136            self.processed_sensors.pitot = latest_pitot;
137        }
138        if !had_raw_range {
139            self.processed_sensors.range = latest_range;
140        }
141        if !had_raw_gnss {
142            self.processed_sensors.gnss = latest_gnss;
143        }
144        if !had_raw_battery {
145            self.processed_sensors.battery = latest_battery;
146        }
147        if !had_raw_rc {
148            self.processed_sensors.rc = latest_rc;
149        }
150        if !had_raw_attitude {
151            self.processed_sensors.attitude = latest_attitude;
152        }
153
154        self.update_sensor_health_and_calibration(now_us);
155        WorldReport {
156            had_raw_sensor: had_raw_imu
157                || had_raw_mag
158                || had_raw_baro
159                || had_raw_pitot
160                || had_raw_range
161                || had_raw_gnss
162                || had_raw_battery
163                || had_raw_rc
164                || had_raw_attitude,
165            had_raw_imu,
166            had_raw_baro,
167            had_raw_rc,
168            had_processed_imu: self.processed_sensors.imu.is_some(),
169            had_processed_baro: self.processed_sensors.baro.is_some(),
170            had_processed_rc: self.processed_sensors.rc.is_some(),
171            ..WorldReport::default()
172        }
173    }
174
175    pub fn run_communication_and_parameter_service_stage(&mut self) {
176        self.process_comm_stage();
177        if self.has_pending_companion_work() {
178            self.apply_companion_events();
179        }
180        if !self.command_events.is_empty() {
181            self.apply_command_events();
182        }
183        if self.has_pending_param_work() {
184            self.service_param_events();
185        }
186        self.request_gyro_calibration_if_needed();
187        if self.param_events.full_refresh || !self.param_events.changes.is_empty() {
188            self.apply_param_reactions();
189        }
190    }
191
192    pub fn run_sensor_ingestion_and_health_stage(&mut self) {
193        self.run_sensor_ingestion_and_health_stage_without_log_drain();
194        self.drain_logs_and_send_responses();
195    }
196
197    pub(super) fn run_sensor_ingestion_and_health_stage_without_log_drain(&mut self) {
198        let now_us = self.board.clock_micros();
199
200        self.run_sensor_ingestion_stage();
201        self.update_sensor_health_and_calibration(now_us);
202    }
203
204    pub(super) fn process_comm_stage(&mut self) {
205        self.comm.process_incoming_messages(&mut self.board);
206        if !self.comm.has_pending_messages() {
207            return;
208        }
209        self.comm.act_on_messages(
210            &mut self.param_events,
211            &mut self.comm_events,
212            &mut self.command_events,
213            &mut self.companion_events,
214            &mut self.board,
215        );
216    }
217
218    pub(super) fn has_pending_companion_work(&self) -> bool {
219        !self.companion_events.is_empty()
220            || (self.companion_link.connected && self.pending_hard_error.is_some())
221    }
222
223    pub(super) fn has_pending_param_work(&self) -> bool {
224        !self.param_events.set_requests.is_empty()
225            || !self.param_events.read_requests.is_empty()
226            || !self.param_events.list_requests.is_empty()
227            || self.param_list_state.is_active()
228    }
229
230    pub(super) fn apply_companion_events(&mut self) {
231        companion::apply_companion_inputs(&mut CompanionInputCtx {
232            events: &mut self.companion_events,
233            comm_events: &mut self.comm_events,
234            link: &mut self.companion_link,
235            aux_commands: &mut self.aux_commands,
236            external_attitude: &mut self.external_attitude,
237            pending_hard_error: &mut self.pending_hard_error,
238        });
239    }
240
241    pub(super) fn apply_command_events(&mut self) {
242        command_service::apply_command_requests(&mut CommandRequestCtx {
243            requests: &mut self.command_events,
244            param_events: &mut self.param_events,
245            comm_events: &mut self.comm_events,
246            state: &self.state,
247            command: &mut self.command,
248            controller: &mut self.controller,
249            board: &mut self.board,
250            flags: &mut self.cal_flags,
251            params: &mut self.params,
252        });
253    }
254
255    pub(super) fn service_param_events(&mut self) {
256        param_service::service_param_events(&mut ParamServiceCtx {
257            params: &mut self.params,
258            state: &mut self.param_list_state,
259            events: &mut self.param_events,
260            comm_events: &mut self.comm_events,
261        });
262    }
263
264    pub(super) fn apply_param_reactions(&mut self) {
265        if self.param_events.full_refresh {
266            self.comm.configure_telemetry_from_params(&self.params);
267        } else {
268            let now_us = self.board.clock_micros();
269            for change in self.param_events.changes.iter() {
270                self.comm
271                    .update_telemetry_param(&self.params, change.id, now_us);
272            }
273        }
274        reactions::apply_param_reactions(&mut ParamReactionCtx {
275            events: &mut self.param_events,
276            params: &self.params,
277            rc: &mut self.rc,
278            command: &mut self.command,
279            state: &mut self.state,
280            estimator: &mut self.estimator,
281            controller: &mut self.controller,
282            mixer: &mut self.mixer,
283            control_pipeline: &mut self.control_pipeline,
284        });
285    }
286
287    pub(super) fn request_gyro_calibration_if_needed(&mut self) {
288        if self.state.is_calibrating() && !self.cal_flags.contains(CalibrationFlags::GYRO) {
289            self.cal_flags.remove(CalibrationFlags::GYRO_FAILED);
290            param_service::set_param_and_emit_change(
291                &mut self.params,
292                &mut self.param_events.changes,
293                ParamId::PARAM_GYRO_X_BIAS,
294                ParamValue::Float(0.0),
295            );
296            param_service::set_param_and_emit_change(
297                &mut self.params,
298                &mut self.param_events.changes,
299                ParamId::PARAM_GYRO_Y_BIAS,
300                ParamValue::Float(0.0),
301            );
302            param_service::set_param_and_emit_change(
303                &mut self.params,
304                &mut self.param_events.changes,
305                ParamId::PARAM_GYRO_Z_BIAS,
306                ParamValue::Float(0.0),
307            );
308            self.cal_flags.insert(CalibrationFlags::GYRO);
309        }
310    }
311
312    pub(super) fn process_sensor_bus_after_update(&mut self) {
313        let calibration_flags_before = self.cal_flags;
314        let baro_bias_before = self.params.get_by_id(ParamId::PARAM_BARO_BIAS);
315        let ground_level_before = self.params.get_by_id(ParamId::PARAM_GROUND_LEVEL);
316        process_sensor_bus(SensorIngestionCtx {
317            raw: &mut self.raw_sensors,
318            processed: &mut self.processed_sensors,
319            processors: &mut self.sensor_processors,
320            flags: &mut self.cal_flags,
321            params: &mut self.params,
322        });
323        if calibration_flags_before.contains(CalibrationFlags::BARO)
324            && !self.cal_flags.contains(CalibrationFlags::BARO)
325            && !self.cal_flags.contains(CalibrationFlags::BARO_FAILED)
326        {
327            // The barometer processor owns the asynchronous sampling window
328            // and writes these values directly.  Publish them now so
329            // rosflight_io receives the completion acknowledgement as the
330            // normal MAVLink parameter update.
331            param_service::emit_param_change(
332                &mut self.param_events.changes,
333                ParamId::PARAM_BARO_BIAS,
334                baro_bias_before,
335                self.params.get_by_id(ParamId::PARAM_BARO_BIAS),
336            );
337            param_service::emit_param_change(
338                &mut self.param_events.changes,
339                ParamId::PARAM_GROUND_LEVEL,
340                ground_level_before,
341                self.params.get_by_id(ParamId::PARAM_GROUND_LEVEL),
342            );
343        }
344        if calibration_flags_before.contains(CalibrationFlags::GYRO)
345            && !self.cal_flags.contains(CalibrationFlags::GYRO)
346            && !self.cal_flags.contains(CalibrationFlags::GYRO_FAILED)
347        {
348            self.estimator.reset_adaptive_bias();
349        }
350        if calibration_flags_before.contains(CalibrationFlags::ACCEL)
351            && !self.cal_flags.contains(CalibrationFlags::ACCEL)
352            && !self.cal_flags.contains(CalibrationFlags::ACCEL_FAILED)
353        {
354            self.estimator.reset();
355            self.control_pipeline = ControlPipelineResource::default();
356        }
357    }
358
359    pub(super) fn process_imu_sensor_after_update(&mut self) {
360        let calibration_flags_before = self.cal_flags;
361        process_imu_sensor(SensorIngestionCtx {
362            raw: &mut self.raw_sensors,
363            processed: &mut self.processed_sensors,
364            processors: &mut self.sensor_processors,
365            flags: &mut self.cal_flags,
366            params: &mut self.params,
367        });
368        if calibration_flags_before.contains(CalibrationFlags::GYRO)
369            && !self.cal_flags.contains(CalibrationFlags::GYRO)
370            && !self.cal_flags.contains(CalibrationFlags::GYRO_FAILED)
371        {
372            self.estimator.reset_adaptive_bias();
373        }
374        if calibration_flags_before.contains(CalibrationFlags::ACCEL)
375            && !self.cal_flags.contains(CalibrationFlags::ACCEL)
376            && !self.cal_flags.contains(CalibrationFlags::ACCEL_FAILED)
377        {
378            self.estimator.reset();
379            self.control_pipeline = ControlPipelineResource::default();
380        }
381    }
382
383    pub(super) fn record_control_imu_candidate(&mut self) {
384        if let Some(imu) = self.processed_sensors.imu {
385            self.control_imu_accumulator.push(imu);
386        }
387    }
388
389    pub(super) fn run_sensor_ingestion_stage(&mut self) {
390        self.board.update_sensor_bus(&mut self.raw_sensors);
391        self.process_sensor_bus_after_update();
392    }
393
394    pub(super) fn drain_logs_and_send_responses(&mut self) {
395        log_drain::drain_logs_to_comm_responses(LogDrainCtx {
396            responses: EventEmitPort::new(&mut self.comm_events.responses),
397            connected: self.companion_link.connected,
398        });
399        if self.comm_events.is_empty() {
400            return;
401        }
402        self.comm
403            .send_comm_responses(&mut self.board, &mut self.comm_events);
404    }
405
406    pub(super) fn drain_logs_and_send_responses_limited(&mut self, max_responses: usize) {
407        log_drain::drain_logs_to_comm_responses(LogDrainCtx {
408            responses: EventEmitPort::new(&mut self.comm_events.responses),
409            connected: self.companion_link.connected,
410        });
411        if self.comm_events.is_empty() {
412            return;
413        }
414        self.comm.send_comm_responses_limited(
415            &mut self.board,
416            &mut self.comm_events,
417            max_responses,
418        );
419    }
420
421    pub(super) fn update_sensor_health_and_calibration(&mut self, now_us: u64) {
422        update_sensor_health(SensorHealthCtx {
423            now_us,
424            sensors: &self.processed_sensors,
425            params: &self.params,
426            state: &mut self.state,
427            last_imu_seen: &mut self.last_imu_seen,
428            imu_timeout_us: IMU_TIMEOUT_US,
429        });
430
431        let failed_arm_calibration =
432            self.state.is_calibrating() && self.cal_flags.contains(CalibrationFlags::GYRO_FAILED);
433        let completed_arm_calibration =
434            self.state.is_calibrating() && !self.cal_flags.contains(CalibrationFlags::GYRO);
435
436        if failed_arm_calibration {
437            self.state.update(Event::CALIBRATION_FAILED, &self.params);
438            self.cal_flags.remove(CalibrationFlags::GYRO_FAILED);
439        } else if completed_arm_calibration {
440            self.state.update(Event::CALIBRATION_COMPLETE, &self.params);
441        }
442    }
443}
veloxity_core/world/service.rs

veloxity_core/world/
service.rs
