1use crate::{
2 board::BoardIo,
3 command::CommandManager,
4 companion::{AuxCommandState, ExternalAttitudeState},
5 controller::{Controller, ControllerCtx, RcTrimCalibrator},
6 estimator::{AttitudeEstimate, Estimator, EstimatorCtx},
7 math::FlightFloat,
8 mixer::{Mixer, MixerCtx, MixerStatus},
9 params::Params,
10 pwm::PwmDriver,
11 pwm::output_sync::{PwmOutputState, compose_pwm_outputs, write_pwm_commands},
12 sensors::ProcessedSensors,
13 state_machine::{ErrorFlag, StateManager},
14};
15
16#[cfg(any(
17 all(
18 feature = "control-scope-estimator",
19 feature = "control-scope-controller"
20 ),
21 all(feature = "control-scope-estimator", feature = "control-scope-mixer"),
22 all(feature = "control-scope-estimator", feature = "control-scope-pwm"),
23 all(feature = "control-scope-controller", feature = "control-scope-mixer"),
24 all(feature = "control-scope-controller", feature = "control-scope-pwm"),
25 all(feature = "control-scope-mixer", feature = "control-scope-pwm"),
26))]
27compile_error!("Enable only one control-scope-* feature at a time");
28
29pub struct ControlPipelineResource<S, A, R: FlightFloat> {
30 pub latest_estimator_state: S,
31 pub latest_actuator_commands: Option<A>,
32 pub latest_pwm_outputs: [R; crate::pwm::output_sync::PWM_OUTPUT_CHANNELS],
33 pub latest_loop_time_us: u16,
34 last_imu_time: u64,
35 pwm_rates_configured: bool,
36}
37
38impl<S, A, R> Default for ControlPipelineResource<S, A, R>
39where
40 S: Default,
41 R: FlightFloat,
42{
43 fn default() -> Self {
44 Self {
45 latest_estimator_state: Default::default(),
46 latest_actuator_commands: None,
47 latest_pwm_outputs: [<R as FlightFloat>::from_f32(0.0);
48 crate::pwm::output_sync::PWM_OUTPUT_CHANNELS],
49 latest_loop_time_us: 0,
50 last_imu_time: 0,
51 pwm_rates_configured: false,
52 }
53 }
54}
55
56#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
57pub struct ControlPipelineTiming {
58 pub estimator_us: u16,
59 pub controller_us: u16,
60 pub mixer_us: u16,
61 pub pwm_us: u16,
62}
63
64impl<S, A, R: FlightFloat> ControlPipelineResource<S, A, R> {
65 pub(crate) fn last_imu_time(&self) -> u64 {
66 self.last_imu_time
67 }
68
69 pub(crate) fn set_last_imu_time(&mut self, timestamp: u64) {
70 self.last_imu_time = timestamp;
71 }
72
73 pub(crate) fn pwm_rates_configured(&self) -> bool {
74 self.pwm_rates_configured
75 }
76
77 pub(crate) fn set_pwm_rates_configured(&mut self) {
78 self.pwm_rates_configured = true;
79 }
80
81 pub(crate) fn invalidate_pwm_rates(&mut self) {
82 self.pwm_rates_configured = false;
83 }
84
85 pub(crate) fn set_latest(
86 &mut self,
87 state: S,
88 actuator_commands: A,
89 pwm_outputs: [R; crate::pwm::output_sync::PWM_OUTPUT_CHANNELS],
90 loop_time_us: u16,
91 ) {
92 self.latest_estimator_state = state;
93 self.latest_actuator_commands = Some(actuator_commands);
94 self.latest_pwm_outputs = pwm_outputs;
95 self.latest_loop_time_us = loop_time_us;
96 }
97}
98
99pub struct ControlPipelineCtx<'a, B, E, C, M, PD, R: FlightFloat>
100where
101 B: BoardIo,
102 E: Estimator<R>,
103 C: Controller<R, State = E::State> + RcTrimCalibrator,
104 M: Mixer<R, MixerInput = C::ControlOutput>,
105 M::ActuatorCommands: AsRef<[R]> + Copy,
106 E::State: Copy + Default,
107 PD: PwmDriver<R>,
108{
109 pub board: &'a mut B,
110 pub params: &'a Params,
111 pub sensors: &'a ProcessedSensors<R>,
112 pub external_attitude: &'a mut ExternalAttitudeState,
113 pub aux_commands: &'a AuxCommandState,
114 pub command: &'a CommandManager,
115 pub state: &'a mut StateManager,
116 pub estimator: &'a mut E,
117 pub controller: &'a mut C,
118 pub mixer: &'a mut M,
119 pub control_pipeline: &'a mut ControlPipelineResource<E::State, M::ActuatorCommands, R>,
120 pub pwm_output: &'a PwmOutputState,
121 pub pwm: &'a mut PD,
122 pub timing: Option<&'a mut ControlPipelineTiming>,
123}
124
125#[inline(always)]
126fn control_scope_estimator<B: BoardIo>(board: &mut B, high: bool) {
127 #[cfg(feature = "control-scope-estimator")]
128 board.set_test_pin_3(high);
129 #[cfg(not(feature = "control-scope-estimator"))]
130 let _ = (board, high);
131}
132
133#[inline(always)]
134fn control_scope_controller<B: BoardIo>(board: &mut B, high: bool) {
135 #[cfg(feature = "control-scope-controller")]
136 board.set_test_pin_3(high);
137 #[cfg(not(feature = "control-scope-controller"))]
138 let _ = (board, high);
139}
140
141#[inline(always)]
142fn control_scope_mixer<B: BoardIo>(board: &mut B, high: bool) {
143 #[cfg(feature = "control-scope-mixer")]
144 board.set_test_pin_3(high);
145 #[cfg(not(feature = "control-scope-mixer"))]
146 let _ = (board, high);
147}
148
149#[inline(always)]
150fn control_scope_pwm<B: BoardIo>(board: &mut B, high: bool) {
151 #[cfg(feature = "control-scope-pwm")]
152 board.set_test_pin_3(high);
153 #[cfg(not(feature = "control-scope-pwm"))]
154 let _ = (board, high);
155}
156
157pub fn run_control_pipeline_if_new_imu<B, E, C, M, PD, R>(
158 mut ctx: ControlPipelineCtx<'_, B, E, C, M, PD, R>,
159) -> bool
160where
161 B: BoardIo,
162 E: Estimator<R>,
163 C: Controller<R, State = E::State> + RcTrimCalibrator,
164 M: Mixer<R, MixerInput = C::ControlOutput>,
165 M::ActuatorCommands: AsRef<[R]> + Copy,
166 E::State: Copy + Default,
167 PD: PwmDriver<R>,
168 R: FlightFloat,
169{
170 let Some(imu_packet) = ctx.sensors.imu else {
171 return false;
172 };
173
174 let current_time = imu_packet.header.timestamp;
175 let last_imu_time = ctx.control_pipeline.last_imu_time();
176 if last_imu_time == 0 {
177 ctx.control_pipeline.set_last_imu_time(current_time);
178 return false;
179 }
180
181 if current_time <= last_imu_time {
182 ctx.state
183 .set_error_flag(ErrorFlag::TIME_GOING_BACKWARDS, true, ctx.params);
184 return false;
185 }
186 ctx.control_pipeline.set_last_imu_time(current_time);
187 ctx.state
188 .set_error_flag(ErrorFlag::TIME_GOING_BACKWARDS, false, ctx.params);
189 #[cfg(not(feature = "scope-timing-pins"))]
190 ctx.board.set_test_pin_2(true);
191
192 let dt = <R as FlightFloat>::from_u64(current_time.saturating_sub(last_imu_time))
193 * <R as FlightFloat>::from_f32(1e-6);
194
195 let loop_start_us = ctx.board.clock_micros();
196 let external_attitude = ctx.external_attitude.latest.take();
197 let estimator_start_us = ctx.timing.is_some().then(|| ctx.board.clock_micros());
198 control_scope_estimator(ctx.board, true);
199 let state = ctx.estimator.estimate(EstimatorCtx {
200 sensors: ctx.sensors,
201 params: ctx.params,
202 dt,
203 external_attitude,
204 });
205 control_scope_estimator(ctx.board, false);
206 if let Some(estimator_start_us) = estimator_start_us {
207 let elapsed_us = elapsed_u16(estimator_start_us, ctx.board.clock_micros());
208 if let Some(timing) = &mut ctx.timing {
209 timing.estimator_us = elapsed_us;
210 }
211 }
212
213 if state.is_healthy() {
214 ctx.state
215 .set_error_flag(ErrorFlag::UNHEALTHY_ESTIMATOR, false, ctx.params);
216 } else {
217 ctx.state
218 .set_error_flag(ErrorFlag::UNHEALTHY_ESTIMATOR, true, ctx.params);
219 }
220
221 let air_density = ctx.sensors.air_density();
222 let controller_start_us = ctx.timing.is_some().then(|| ctx.board.clock_micros());
223 control_scope_controller(ctx.board, true);
224 let controls = ctx.controller.control(
225 &state,
226 ControllerCtx {
227 state_manager: ctx.state,
228 command: ctx.command.combined_control(),
229 params: ctx.params,
230 air_density,
231 dt,
232 },
233 );
234 control_scope_controller(ctx.board, false);
235 if let Some(controller_start_us) = controller_start_us {
236 let elapsed_us = elapsed_u16(controller_start_us, ctx.board.clock_micros());
237 if let Some(timing) = &mut ctx.timing {
238 timing.controller_us = elapsed_us;
239 }
240 }
241
242 let mixer_start_us = ctx.timing.is_some().then(|| ctx.board.clock_micros());
243 control_scope_mixer(ctx.board, true);
244 let mixer_run = ctx.mixer.mix(
245 &controls,
246 MixerCtx {
247 state: ctx.state,
248 params: ctx.params,
249 rc_override: ctx.command.get_rc_override(),
250 air_density,
251 battery_voltage: ctx
252 .sensors
253 .battery
254 .map(|battery| <R as FlightFloat>::from_f32(battery.voltage)),
255 },
256 );
257 control_scope_mixer(ctx.board, false);
258 match mixer_run.status {
259 MixerStatus::Healthy => {
260 ctx.state
261 .set_error_flag(ErrorFlag::INVALID_MIXER, false, ctx.params)
262 }
263 MixerStatus::InvalidMixer => {
264 ctx.state
265 .set_error_flag(ErrorFlag::INVALID_MIXER, true, ctx.params)
266 }
267 }
268 if let Some(mixer_start_us) = mixer_start_us {
269 let elapsed_us = elapsed_u16(mixer_start_us, ctx.board.clock_micros());
270 if let Some(timing) = &mut ctx.timing {
271 timing.mixer_us = elapsed_us;
272 }
273 }
274
275 let pwm_start_us = ctx.timing.is_some().then(|| ctx.board.clock_micros());
276 let actuator_commands = mixer_run.commands;
277 control_scope_pwm(ctx.board, true);
278 if !ctx.control_pipeline.pwm_rates_configured() {
279 if ctx
280 .pwm
281 .configure_output_rates(ctx.mixer.default_pwm_rates())
282 .is_ok()
283 {
284 ctx.control_pipeline.set_pwm_rates_configured();
285 } else {
286 crate::log_warn!("PWM driver rejected mixer default output rates");
287 }
288 }
289 let pwm_outputs = compose_pwm_outputs(
290 actuator_commands.as_ref(),
291 ctx.mixer.output_types(),
292 ctx.aux_commands.latest.as_ref(),
293 ctx.state,
294 ctx.params,
295 );
296 if let Err(error) = write_pwm_commands(
297 ctx.board,
298 ctx.pwm,
299 ctx.pwm_output,
300 &pwm_outputs,
301 ctx.mixer.output_types(),
302 ctx.state,
303 ) {
304 crate::log_warn!("PWM driver rejected output command: {:?}", error);
305 }
306 control_scope_pwm(ctx.board, false);
307 if let Some(pwm_start_us) = pwm_start_us {
308 let elapsed_us = elapsed_u16(pwm_start_us, ctx.board.clock_micros());
309 if let Some(timing) = &mut ctx.timing {
310 timing.pwm_us = elapsed_us;
311 }
312 }
313 let loop_time_us = ctx
314 .board
315 .clock_micros()
316 .saturating_sub(loop_start_us)
317 .min(u16::MAX as u64) as u16;
318 ctx.control_pipeline
319 .set_latest(state, actuator_commands, pwm_outputs, loop_time_us);
320 #[cfg(not(feature = "scope-timing-pins"))]
321 ctx.board.set_test_pin_2(false);
322 true
323}
324
325fn elapsed_u16(start_us: u64, end_us: u64) -> u16 {
326 end_us.saturating_sub(start_us).min(u16::MAX as u64) as u16
327}