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, 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::{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}
167
168impl RealtimeServicePolicy {
169 pub const fn with_spacing(min_spacing_us: u64, telemetry_streams_per_phase: usize) -> Self {
170 Self {
171 min_spacing_us,
172 telemetry_streams_per_phase,
173 continue_when_idle: false,
174 }
175 }
176
177 pub const fn continuous(telemetry_streams_per_phase: usize) -> Self {
178 Self {
179 min_spacing_us: 0,
180 telemetry_streams_per_phase,
181 continue_when_idle: false,
182 }
183 }
184
185 pub const fn continuous_polling(telemetry_streams_per_phase: usize) -> Self {
186 Self {
187 min_spacing_us: 0,
188 telemetry_streams_per_phase,
189 continue_when_idle: true,
190 }
191 }
192}
193
194#[derive(Clone, Copy, Debug, PartialEq, Eq)]
195pub struct ControlLoopRates {
196 pub control_hz: u16,
199}
200
201impl ControlLoopRates {
202 pub const fn every_imu_sample() -> Self {
203 Self { control_hz: 0 }
204 }
205
206 pub const fn fixed_rate_hz(control_hz: u16) -> Self {
207 Self { control_hz }
208 }
209}
210
211impl Default for ControlLoopRates {
212 fn default() -> Self {
213 Self::every_imu_sample()
214 }
215}
216
217pub struct World<B, E, C, M, CI, PD, R: FlightFloat>
218where
219 B: BoardIo,
220 E: Estimator<R>,
221 C: Controller<R, State = E::State> + RcTrimCalibrator,
222 M: crate::mixer::Mixer<R, MixerInput = C::ControlOutput>,
223 M::ActuatorCommands: AsRef<[R]> + Copy,
224 E::State: Copy + Default,
225 CI: CommInterface<B>,
226 PD: PwmDriver<R>,
227{
228 board: B,
229 params: Params,
230 param_list_state: ParamListState,
231 param_events: ParamEventQueues,
232 comm_events: CommEventQueues,
233 command_events: CommandEventQueues,
234 companion_events: CompanionEventQueues,
235 companion_link: CompanionLinkState,
236 pending_hard_error: Option<RosflightHardErrorMsg>,
237 aux_commands: AuxCommandState,
238 external_attitude: ExternalAttitudeState,
239 comm: CommManager<B, CI>,
240 raw_sensors: SensorBus<R>,
241 processed_sensors: ProcessedSensors<R>,
242 control_imu_accumulator: ImuSampleAccumulator<R>,
243 sensor_processors: SensorProcessorSet<R>,
244 rc: Rc,
245 command: CommandManager,
246 state: StateManager,
247 cal_flags: CalibrationFlags,
248 estimator: E,
249 controller: C,
250 mixer: M,
251 control_pipeline: ControlPipelineResource<E::State, M::ActuatorCommands, R>,
252 pwm_output: PwmOutputState,
253 pwm: PD,
254 last_imu_seen: u64,
255 control_loop_rates: ControlLoopRates,
256 last_control_update_us: u64,
257 last_realtime_control_us: u64,
258 next_realtime_service_us: u64,
259}
260
261impl<B, E, C, M, CI, PD, R> World<B, E, C, M, CI, PD, R>
262where
263 B: BoardIo,
264 E: Estimator<R>,
265 C: Controller<R, State = E::State> + RcTrimCalibrator,
266 M: crate::mixer::Mixer<R, MixerInput = C::ControlOutput>,
267 M::ActuatorCommands: AsRef<[R]> + Copy,
268 E::State: Copy + Default,
269 CI: CommInterface<B>,
270 PD: PwmDriver<R>,
271 R: FlightFloat,
272{
273 pub fn init(
274 mut board: B,
275 mut params: Params,
276 comm_link: CI,
277 mut state: StateManager,
278 estimator: E,
279 controller: C,
280 mixer: M,
281 pwm: PD,
282 ) -> Self {
283 crate::mixer::matrix::sync_reflected_mixer_params(
284 &mut params,
285 ParamId::PARAM_PRIMARY_MIXER,
286 );
287 crate::mixer::matrix::sync_reflected_mixer_params(
288 &mut params,
289 ParamId::PARAM_SECONDARY_MIXER,
290 );
291
292 state.update(Event::INITIALIZED, ¶ms);
293
294 let mut rc = Rc::new();
295 rc.init(¶ms);
296
297 let mut command = CommandManager::new();
298 command.init(¶ms, &mut state);
299
300 let now_us = board.clock_micros();
301 let mut comm = CommManager::new(comm_link, now_us);
302 comm.configure_telemetry_from_params(¶ms);
303
304 let pwm_output = PwmOutputState::new(pwm.is_enabled());
305
306 let pending_hard_error = board.backup_memory_read().map(|data| {
307 let _ = board.backup_memory_clear();
308 RosflightHardErrorMsg {
309 error_code: data.error_code,
310 pc: data.pc,
311 reset_count: data.reset_count,
312 do_rearm: data.do_rearm,
313 }
314 });
315 let do_rearm_after_hardfault = pending_hard_error
316 .as_ref()
317 .map(|msg| msg.do_rearm != 0)
318 .unwrap_or(false);
319
320 let mut world = Self {
321 board,
322 params,
323 param_list_state: ParamListState::default(),
324 param_events: ParamEventQueues::default(),
325 comm_events: CommEventQueues::default(),
326 command_events: CommandEventQueues::default(),
327 companion_events: CompanionEventQueues::default(),
328 companion_link: CompanionLinkState::default(),
329 pending_hard_error,
330 aux_commands: AuxCommandState::default(),
331 external_attitude: ExternalAttitudeState::default(),
332 comm,
333 raw_sensors: SensorBus::default(),
334 processed_sensors: ProcessedSensors::default(),
335 control_imu_accumulator: ImuSampleAccumulator::default(),
336 sensor_processors: SensorProcessorSet::default(),
337 rc,
338 command,
339 state,
340 cal_flags: CalibrationFlags::empty(),
341 estimator,
342 controller,
343 mixer,
344 control_pipeline: ControlPipelineResource::default(),
345 pwm_output,
346 pwm,
347 last_imu_seen: now_us,
348 control_loop_rates: ControlLoopRates::default(),
349 last_control_update_us: now_us,
350 last_realtime_control_us: now_us,
351 next_realtime_service_us: now_us,
352 };
353 if do_rearm_after_hardfault {
354 world
355 .state
356 .update(Event::HARDFAULT_REARM_REQUESTED, &world.params);
357 }
358 world.estimator.update_params(&world.params);
359 world.controller.update_gains(&world.params);
360 world
361 }
362}
363
364mod control;
365mod service;
366mod telemetry;
367#[cfg(test)]
368mod tests;