veloxity_core/
world.rs

1use crate::{
2    board::BoardIo,
3    comm::messages::messages::RosflightHardErrorMsg,
4    comm::{
5        CommManager, RealtimeTelemetryPriority, TelemetryCtx, TelemetryRates,
6        interface::CommInterface,
7    },
8    command::CommandManager,
9    command::service::{self as command_service, CommandRequestCtx},
10    companion::{
11        self, AuxCommandState, CompanionInputCtx, CompanionLinkState, ExternalAttitudeState,
12    },
13    control::{
14        ControlPipelineCtx, ControlPipelineResource, ControlPipelineTiming,
15        run_control_pipeline_if_new_imu,
16    },
17    controller::{Controller, RcTrimCalibrator},
18    estimator::Estimator,
19    events::{CommEventQueues, CommandEventQueues, CompanionEventQueues, ParamEventQueues},
20    log::drain::{self as log_drain, LogDrainCtx},
21    math::FlightFloat,
22    params::reactions::{self, ParamReactionCtx},
23    params::service::{self as param_service, ParamListState, ParamServiceCtx},
24    params::{ParamId, ParamValue, Params},
25    ports::EventEmitPort,
26    pwm::PwmDriver,
27    pwm::output_sync::{PwmOutputState, PwmSyncCtx, sync_pwm_output_state},
28    rc::Rc,
29    rc::command_state::{RcCommandStateCtx, run_rc_command_state},
30    sensors::health::{SensorHealthCtx, initialize_imu_calibration_error, update_sensor_health},
31    sensors::ingestion::{
32        SensorIngestionCtx, SensorProcessorSet, process_imu_sensor, process_sensor_bus,
33    },
34    sensors::processors::CalibrationFlags,
35    sensors::{ProcessedSensors, SensorBus},
36    state_machine::{ErrorFlag, Event, StateManager},
37};
38
39const IMU_TIMEOUT_US: u64 = 100_000;
40const REALTIME_SERVICE_RESPONSE_BUDGET: usize = 1;
41const REALTIME_SERVICE_MIN_CONTROL_SLACK_US: u64 = 200;
42
43#[derive(Clone, Copy, Debug)]
44struct ImuSampleAccumulator<R: FlightFloat> {
45    accel_sum: [R; 3],
46    gyro_sum: [R; 3],
47    temperature_sum: f32,
48    count: u16,
49    latest_header: crate::packets::RosflightPacketHeader,
50    latest_seq: u32,
51}
52
53impl<R: FlightFloat> Default for ImuSampleAccumulator<R> {
54    fn default() -> Self {
55        Self {
56            accel_sum: [<R as FlightFloat>::from_f32(0.0); 3],
57            gyro_sum: [<R as FlightFloat>::from_f32(0.0); 3],
58            temperature_sum: 0.0,
59            count: 0,
60            latest_header: crate::packets::RosflightPacketHeader::default(),
61            latest_seq: 0,
62        }
63    }
64}
65
66impl<R: FlightFloat> ImuSampleAccumulator<R> {
67    fn has_samples(&self) -> bool {
68        self.count != 0
69    }
70
71    fn push(&mut self, sample: crate::packets::ImuPacket<R>) {
72        self.accel_sum[0] += sample.accel[0];
73        self.accel_sum[1] += sample.accel[1];
74        self.accel_sum[2] += sample.accel[2];
75        self.gyro_sum[0] += sample.gyro[0];
76        self.gyro_sum[1] += sample.gyro[1];
77        self.gyro_sum[2] += sample.gyro[2];
78        self.temperature_sum += sample.temperature;
79        self.count = self.count.saturating_add(1);
80        self.latest_header = sample.header;
81        self.latest_seq = sample.seq;
82    }
83
84    fn take_average(&mut self) -> Option<crate::packets::ImuPacket<R>> {
85        if self.count == 0 {
86            return None;
87        }
88        let count = <R as FlightFloat>::from_u64(self.count as u64);
89        let temperature_count = self.count as f32;
90        let sample = crate::packets::ImuPacket {
91            header: self.latest_header,
92            accel: [
93                self.accel_sum[0] / count,
94                self.accel_sum[1] / count,
95                self.accel_sum[2] / count,
96            ],
97            gyro: [
98                self.gyro_sum[0] / count,
99                self.gyro_sum[1] / count,
100                self.gyro_sum[2] / count,
101            ],
102            temperature: self.temperature_sum / temperature_count,
103            seq: self.latest_seq,
104        };
105        *self = Self::default();
106        Some(sample)
107    }
108}
109
110#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
111pub struct WorldReport {
112    pub had_rx: bool,
113    pub had_raw_sensor: bool,
114    pub had_raw_imu: bool,
115    pub had_raw_baro: bool,
116    pub had_raw_rc: bool,
117    pub had_processed_imu: bool,
118    pub had_processed_baro: bool,
119    pub had_processed_rc: bool,
120    pub telemetry_due: bool,
121    pub telemetry_deferred: bool,
122    pub ran_control: bool,
123    pub elapsed_after_control_us: u32,
124    pub estimator_us: u16,
125    pub controller_us: u16,
126    pub mixer_us: u16,
127    pub pwm_us: u16,
128}
129
130impl WorldReport {
131    fn merge_from(&mut self, other: Self) {
132        self.had_rx |= other.had_rx;
133        self.had_raw_sensor |= other.had_raw_sensor;
134        self.had_raw_imu |= other.had_raw_imu;
135        self.had_raw_baro |= other.had_raw_baro;
136        self.had_raw_rc |= other.had_raw_rc;
137        self.had_processed_imu |= other.had_processed_imu;
138        self.had_processed_baro |= other.had_processed_baro;
139        self.had_processed_rc |= other.had_processed_rc;
140        self.telemetry_due |= other.telemetry_due;
141        self.telemetry_deferred |= other.telemetry_deferred;
142        self.ran_control |= other.ran_control;
143        self.elapsed_after_control_us = self
144            .elapsed_after_control_us
145            .saturating_add(other.elapsed_after_control_us);
146        self.estimator_us = self.estimator_us.saturating_add(other.estimator_us);
147        self.controller_us = self.controller_us.saturating_add(other.controller_us);
148        self.mixer_us = self.mixer_us.saturating_add(other.mixer_us);
149        self.pwm_us = self.pwm_us.saturating_add(other.pwm_us);
150    }
151}
152
153#[derive(Clone, Copy, Debug, PartialEq, Eq)]
154pub enum RealtimeSchedulerStep {
155    ImuControl,
156    ControlUpdate,
157    Service,
158    Idle,
159}
160
161#[derive(Clone, Copy, Debug, PartialEq, Eq)]
162pub struct RealtimeServicePolicy {
163    pub min_spacing_us: u64,
164    pub telemetry_streams_per_phase: usize,
165    pub continue_when_idle: bool,
166    pub drain_telemetry_with_available_slack: bool,
167}
168
169impl RealtimeServicePolicy {
170    pub const fn with_spacing(min_spacing_us: u64, telemetry_streams_per_phase: usize) -> Self {
171        Self {
172            min_spacing_us,
173            telemetry_streams_per_phase,
174            continue_when_idle: false,
175            drain_telemetry_with_available_slack: false,
176        }
177    }
178
179    pub const fn continuous(telemetry_streams_per_phase: usize) -> Self {
180        Self {
181            min_spacing_us: 0,
182            telemetry_streams_per_phase,
183            continue_when_idle: false,
184            drain_telemetry_with_available_slack: false,
185        }
186    }
187
188    pub const fn continuous_polling(telemetry_streams_per_phase: usize) -> Self {
189        Self {
190            min_spacing_us: 0,
191            telemetry_streams_per_phase,
192            continue_when_idle: true,
193            drain_telemetry_with_available_slack: false,
194        }
195    }
196
197    /// Continuously services the board and sends every due telemetry stream
198    /// that fits inside the measured control-deadline slack.
199    pub const fn continuous_slack_driven() -> Self {
200        Self {
201            min_spacing_us: 0,
202            telemetry_streams_per_phase: 0,
203            continue_when_idle: true,
204            drain_telemetry_with_available_slack: true,
205        }
206    }
207}
208
209#[derive(Clone, Copy, Debug, PartialEq, Eq)]
210pub struct ControlLoopRates {
211    /// Full estimator/controller/mixer/PWM update rate. A value of 0 runs control on every new
212    /// IMU sample.
213    pub control_hz: u16,
214}
215
216impl ControlLoopRates {
217    pub const fn every_imu_sample() -> Self {
218        Self { control_hz: 0 }
219    }
220
221    pub const fn fixed_rate_hz(control_hz: u16) -> Self {
222        Self { control_hz }
223    }
224}
225
226impl Default for ControlLoopRates {
227    fn default() -> Self {
228        Self::every_imu_sample()
229    }
230}
231
232pub struct World<B, E, C, M, CI, PD, R: FlightFloat>
233where
234    B: BoardIo,
235    E: Estimator<R>,
236    C: Controller<R, State = E::State> + RcTrimCalibrator,
237    M: crate::mixer::Mixer<R, MixerInput = C::ControlOutput>,
238    M::ActuatorCommands: AsRef<[R]> + Copy,
239    E::State: Copy + Default,
240    CI: CommInterface<B>,
241    PD: PwmDriver<R>,
242{
243    board: B,
244    params: Params,
245    param_list_state: ParamListState,
246    param_events: ParamEventQueues,
247    comm_events: CommEventQueues,
248    command_events: CommandEventQueues,
249    companion_events: CompanionEventQueues,
250    companion_link: CompanionLinkState,
251    pending_hard_error: Option<RosflightHardErrorMsg>,
252    aux_commands: AuxCommandState,
253    external_attitude: ExternalAttitudeState,
254    comm: CommManager<B, CI>,
255    raw_sensors: SensorBus<R>,
256    processed_sensors: ProcessedSensors<R>,
257    control_imu_accumulator: ImuSampleAccumulator<R>,
258    sensor_processors: SensorProcessorSet<R>,
259    rc: Rc,
260    command: CommandManager,
261    state: StateManager,
262    cal_flags: CalibrationFlags,
263    estimator: E,
264    controller: C,
265    mixer: M,
266    control_pipeline: ControlPipelineResource<E::State, M::ActuatorCommands, R>,
267    pwm_output: PwmOutputState,
268    pwm: PD,
269    last_imu_seen: u64,
270    control_loop_rates: ControlLoopRates,
271    last_control_update_us: u64,
272    last_realtime_control_us: u64,
273    next_realtime_service_us: u64,
274}
275
276impl<B, E, C, M, CI, PD, R> World<B, E, C, M, CI, PD, R>
277where
278    B: BoardIo,
279    E: Estimator<R>,
280    C: Controller<R, State = E::State> + RcTrimCalibrator,
281    M: crate::mixer::Mixer<R, MixerInput = C::ControlOutput>,
282    M::ActuatorCommands: AsRef<[R]> + Copy,
283    E::State: Copy + Default,
284    CI: CommInterface<B>,
285    PD: PwmDriver<R>,
286    R: FlightFloat,
287{
288    pub fn init(
289        mut board: B,
290        mut params: Params,
291        comm_link: CI,
292        mut state: StateManager,
293        estimator: E,
294        controller: C,
295        mixer: M,
296        pwm: PD,
297    ) -> Self {
298        crate::mixer::matrix::sync_reflected_mixer_params(
299            &mut params,
300            ParamId::PARAM_PRIMARY_MIXER,
301        );
302        crate::mixer::matrix::sync_reflected_mixer_params(
303            &mut params,
304            ParamId::PARAM_SECONDARY_MIXER,
305        );
306
307        state.update(Event::INITIALIZED, &params);
308        // Match ROSflight C's Sensors::init_imu(): establish the calibration
309        // interlock from persisted parameters before the first IMU sample or
310        // arming request can be processed.
311        initialize_imu_calibration_error(&mut state, &params);
312
313        let mut rc = Rc::new();
314        rc.init(&params);
315
316        let mut command = CommandManager::new();
317        command.init(&params, &mut state);
318
319        let voltage_multiplier = match params.get_by_id(ParamId::PARAM_BATTERY_VOLTAGE_MULTIPLIER) {
320            ParamValue::Float(value) => value,
321            _ => 0.0,
322        };
323        let current_multiplier = match params.get_by_id(ParamId::PARAM_BATTERY_CURRENT_MULTIPLIER) {
324            ParamValue::Float(value) => value,
325            _ => 0.0,
326        };
327        board.configure_battery_monitor(voltage_multiplier, current_multiplier);
328
329        let now_us = board.clock_micros();
330        let mut comm = CommManager::new(comm_link, now_us);
331        comm.configure_telemetry_from_params(&params);
332
333        let pwm_output = PwmOutputState::new(pwm.is_enabled());
334
335        let pending_hard_error = board.backup_memory_read().map(|data| {
336            let _ = board.backup_memory_clear();
337            RosflightHardErrorMsg {
338                error_code: data.error_code,
339                pc: data.pc,
340                reset_count: data.reset_count,
341                do_rearm: data.do_rearm,
342            }
343        });
344        let do_rearm_after_hardfault = pending_hard_error
345            .as_ref()
346            .map(|msg| msg.do_rearm != 0)
347            .unwrap_or(false);
348
349        let mut world = Self {
350            board,
351            params,
352            param_list_state: ParamListState::default(),
353            param_events: ParamEventQueues::default(),
354            comm_events: CommEventQueues::default(),
355            command_events: CommandEventQueues::default(),
356            companion_events: CompanionEventQueues::default(),
357            companion_link: CompanionLinkState::default(),
358            pending_hard_error,
359            aux_commands: AuxCommandState::default(),
360            external_attitude: ExternalAttitudeState::default(),
361            comm,
362            raw_sensors: SensorBus::default(),
363            processed_sensors: ProcessedSensors::default(),
364            control_imu_accumulator: ImuSampleAccumulator::default(),
365            sensor_processors: SensorProcessorSet::default(),
366            rc,
367            command,
368            state,
369            cal_flags: CalibrationFlags::empty(),
370            estimator,
371            controller,
372            mixer,
373            control_pipeline: ControlPipelineResource::default(),
374            pwm_output,
375            pwm,
376            last_imu_seen: now_us,
377            control_loop_rates: ControlLoopRates::default(),
378            last_control_update_us: now_us,
379            last_realtime_control_us: now_us,
380            next_realtime_service_us: now_us,
381        };
382        if do_rearm_after_hardfault {
383            world
384                .state
385                .update(Event::HARDFAULT_REARM_REQUESTED, &world.params);
386        }
387        world.estimator.update_params(&world.params);
388        world.controller.update_gains(&world.params);
389        world
390    }
391}
392
393mod control;
394mod service;
395mod telemetry;
396#[cfg(test)]
397mod tests;
veloxity_core/world.rs

veloxity_core/
world.rs
