veloxity_core/params/
reactions.rs

1use crate::{
2    command::CommandManager,
3    control::ControlPipelineResource,
4    controller::Controller,
5    estimator::Estimator,
6    events::ParamEventQueues,
7    math::FlightFloat,
8    mixer::Mixer,
9    params::{ParamId, Params},
10    rc::Rc,
11    state_machine::{ErrorFlag, Event, StateManager},
12};
13
14pub struct ParamReactionCtx<'a, E, C, M, R>
15where
16    E: Estimator<R>,
17    C: Controller<R, State = E::State>,
18    M: Mixer<R, MixerInput = C::ControlOutput>,
19    R: FlightFloat,
20{
21    pub events: &'a mut ParamEventQueues,
22    pub params: &'a Params,
23    pub rc: &'a mut Rc,
24    pub command: &'a mut CommandManager,
25    pub state: &'a mut StateManager,
26    pub estimator: &'a mut E,
27    pub controller: &'a mut C,
28    pub mixer: &'a mut M,
29    pub control_pipeline: &'a mut ControlPipelineResource<E::State, M::ActuatorCommands, R>,
30}
31
32pub fn apply_param_reactions<E, C, M, R>(ctx: &mut ParamReactionCtx<'_, E, C, M, R>)
33where
34    E: Estimator<R>,
35    C: Controller<R, State = E::State>,
36    M: Mixer<R, MixerInput = C::ControlOutput>,
37    R: FlightFloat,
38{
39    let full_refresh = ctx.events.full_refresh;
40    let has_param_changes = ctx.events.changes.iter().next().is_some();
41    if full_refresh {
42        mixer_refresh_params(ctx);
43        ctx.rc.init(ctx.params);
44        ctx.command.update_failsafe_config(ctx.params, ctx.state);
45    } else {
46        mixer_on_param_changed(ctx);
47        rc_on_param_changed(ctx);
48        command_on_param_changed(ctx);
49    }
50
51    if full_refresh || has_param_changes {
52        ctx.estimator.update_params(ctx.params);
53        ctx.controller.update_gains(ctx.params);
54    }
55
56    ctx.events.changes.clear();
57    ctx.events.full_refresh = false;
58}
59
60fn mixer_refresh_params<E, C, M, R>(ctx: &mut ParamReactionCtx<'_, E, C, M, R>)
61where
62    E: Estimator<R>,
63    C: Controller<R, State = E::State>,
64    M: Mixer<R, MixerInput = C::ControlOutput>,
65    R: FlightFloat,
66{
67    let Some(status) = ctx.mixer.refresh_params(ctx.params) else {
68        return;
69    };
70    ctx.control_pipeline.invalidate_pwm_rates();
71    match status {
72        crate::mixer::MixerStatus::Healthy => ctx
73            .state
74            .update(Event::ERROR_CLEARED(ErrorFlag::INVALID_MIXER), ctx.params),
75        crate::mixer::MixerStatus::InvalidMixer => ctx
76            .state
77            .update(Event::ERROR_OCCURRED(ErrorFlag::INVALID_MIXER), ctx.params),
78    }
79}
80
81fn mixer_on_param_changed<E, C, M, R>(ctx: &mut ParamReactionCtx<'_, E, C, M, R>)
82where
83    E: Estimator<R>,
84    C: Controller<R, State = E::State>,
85    M: Mixer<R, MixerInput = C::ControlOutput>,
86    R: FlightFloat,
87{
88    for change in ctx.events.changes.iter() {
89        let Some(status) = ctx.mixer.on_param_changed(ctx.params, change.id) else {
90            continue;
91        };
92        ctx.control_pipeline.invalidate_pwm_rates();
93        match status {
94            crate::mixer::MixerStatus::Healthy => ctx
95                .state
96                .update(Event::ERROR_CLEARED(ErrorFlag::INVALID_MIXER), ctx.params),
97            crate::mixer::MixerStatus::InvalidMixer => ctx
98                .state
99                .update(Event::ERROR_OCCURRED(ErrorFlag::INVALID_MIXER), ctx.params),
100        }
101    }
102}
103
104pub fn rc_on_param_changed<E, C, M, R>(ctx: &mut ParamReactionCtx<'_, E, C, M, R>)
105where
106    E: Estimator<R>,
107    C: Controller<R, State = E::State>,
108    M: Mixer<R, MixerInput = C::ControlOutput>,
109    R: FlightFloat,
110{
111    for change in ctx.events.changes.iter() {
112        ctx.rc.param_change_callback(change.id, ctx.params);
113    }
114}
115
116pub fn command_on_param_changed<E, C, M, R>(ctx: &mut ParamReactionCtx<'_, E, C, M, R>)
117where
118    E: Estimator<R>,
119    C: Controller<R, State = E::State>,
120    M: Mixer<R, MixerInput = C::ControlOutput>,
121    R: FlightFloat,
122{
123    for change in ctx.events.changes.iter() {
124        match change.id {
125            ParamId::PARAM_FAILSAFE_THROTTLE | ParamId::PARAM_FIXED_WING => {
126                ctx.command.update_failsafe_config(ctx.params, ctx.state);
127            }
128            _ => {}
129        }
130    }
131}
132
133#[cfg(test)]
134mod tests {
135    use super::*;
136    use crate::{
137        command::CommandManager,
138        control::ControlPipelineResource,
139        events::{ParamChanged, ParamEventQueues},
140        params::{ParamValue, Params},
141        rc::Rc,
142        state_machine::ErrorFlag,
143        vehicle::quadrotor,
144    };
145
146    fn test_ctx<'a>(
147        events: &'a mut ParamEventQueues,
148        params: &'a Params,
149        rc: &'a mut Rc,
150        command: &'a mut CommandManager,
151        state: &'a mut StateManager,
152        estimator: &'a mut quadrotor::Estimator<f64>,
153        controller: &'a mut quadrotor::Controller<f64>,
154        mixer: &'a mut quadrotor::Mixer<f64>,
155        control_pipeline: &'a mut ControlPipelineResource<
156            <quadrotor::Estimator<f64> as Estimator<f64>>::State,
157            <quadrotor::Mixer<f64> as Mixer<f64>>::ActuatorCommands,
158            f64,
159        >,
160    ) -> ParamReactionCtx<
161        'a,
162        quadrotor::Estimator<f64>,
163        quadrotor::Controller<f64>,
164        quadrotor::Mixer<f64>,
165        f64,
166    > {
167        ParamReactionCtx {
168            events,
169            params,
170            rc,
171            command,
172            state,
173            estimator,
174            controller,
175            mixer,
176            control_pipeline,
177        }
178    }
179
180    #[test]
181    fn command_reacts_only_to_failsafe_related_param_changes() {
182        let mut command = CommandManager::new();
183        let mut state = StateManager::new();
184        let mut params = Params::new();
185        let mut events = ParamEventQueues::default();
186        let mut rc = Rc::new();
187        let mut estimator = quadrotor::Estimator::<f64>::default();
188        let mut controller = quadrotor::Controller::<f64>::default();
189        let mut mixer = quadrotor::mixer(&params);
190        let mut control_pipeline = ControlPipelineResource::default();
191
192        params.set_by_id(ParamId::PARAM_FAILSAFE_THROTTLE, ParamValue::Float(2.0));
193        let _ = events.changes.push(ParamChanged {
194            id: ParamId::PARAM_BAUD_RATE,
195            old: ParamValue::Int(921600),
196            new: ParamValue::Int(57600),
197            param_id_bytes: [0; 16],
198        });
199
200        command_on_param_changed(&mut test_ctx(
201            &mut events,
202            &params,
203            &mut rc,
204            &mut command,
205            &mut state,
206            &mut estimator,
207            &mut controller,
208            &mut mixer,
209            &mut control_pipeline,
210        ));
211
212        assert!(!state.get_errors().contains(ErrorFlag::INVALID_FAILSAFE));
213
214        let _ = events.changes.push(ParamChanged {
215            id: ParamId::PARAM_FAILSAFE_THROTTLE,
216            old: ParamValue::Float(0.0),
217            new: ParamValue::Float(2.0),
218            param_id_bytes: [0; 16],
219        });
220
221        command_on_param_changed(&mut test_ctx(
222            &mut events,
223            &params,
224            &mut rc,
225            &mut command,
226            &mut state,
227            &mut estimator,
228            &mut controller,
229            &mut mixer,
230            &mut control_pipeline,
231        ));
232
233        assert!(state.get_errors().contains(ErrorFlag::INVALID_FAILSAFE));
234    }
235
236    #[test]
237    fn full_param_refresh_reacts_once_without_expanding_param_changes() {
238        let mut command = CommandManager::new();
239        let mut state = StateManager::new();
240        let mut params = Params::new();
241        let mut events = ParamEventQueues::default();
242        let mut rc = Rc::new();
243        let mut estimator = quadrotor::Estimator::<f64>::default();
244        let mut controller = quadrotor::Controller::<f64>::default();
245        let mut mixer = quadrotor::mixer(&params);
246        let mut control_pipeline = ControlPipelineResource::default();
247
248        params.set_by_id(ParamId::PARAM_PRIMARY_MIXER, ParamValue::Int(99));
249        events.full_refresh = true;
250
251        apply_param_reactions(&mut test_ctx(
252            &mut events,
253            &params,
254            &mut rc,
255            &mut command,
256            &mut state,
257            &mut estimator,
258            &mut controller,
259            &mut mixer,
260            &mut control_pipeline,
261        ));
262
263        assert!(!events.full_refresh);
264        assert!(events.changes.is_empty());
265        assert!(state.get_errors().contains(ErrorFlag::INVALID_MIXER));
266    }
267}
veloxity_core/params/reactions.rs

veloxity_core/params/
reactions.rs
