veloxity_core/params/
service.rs

1use crate::{
2    comm::messages::{enums::ParamIdentifier, messages::ParamValueMsg},
3    comm::str_to_fixed_bytes,
4    comm::{MAX_TELEMETRY_RATE_HZ, telemetry_stream_for_param},
5    events::{
6        CommEventQueues, CommResponse, EventQueue, PARAM_CHANGED_QUEUE_CAPACITY, ParamChanged,
7        ParamEventQueues,
8    },
9    params::{PARAM_DEFINITIONS, PARAMS_COUNT, ParamId, ParamValue, Params},
10};
11
12#[derive(Default)]
13pub struct ParamListState {
14    next_index: Option<usize>,
15}
16
17impl ParamListState {
18    pub fn is_active(&self) -> bool {
19        self.next_index.is_some()
20    }
21}
22
23pub struct ParamServiceCtx<'a> {
24    pub params: &'a mut Params,
25    pub state: &'a mut ParamListState,
26    pub events: &'a mut ParamEventQueues,
27    pub comm_events: &'a mut CommEventQueues,
28}
29
30pub fn service_param_events(ctx: &mut ParamServiceCtx<'_>) {
31    service_param_read_requests(ctx);
32    service_param_list_requests(ctx);
33    apply_param_requests(ctx);
34}
35
36pub fn apply_param_requests(ctx: &mut ParamServiceCtx<'_>) {
37    while let Some(req) = ctx.events.set_requests.pop() {
38        let Some(id) = param_id_from_name_bytes(req.param_id_bytes) else {
39            continue;
40        };
41
42        let def = &PARAM_DEFINITIONS[id as usize];
43        if !same_param_type(def.default, req.value) {
44            continue;
45        }
46        if telemetry_stream_for_param(id).is_some()
47            && !matches!(req.value, ParamValue::Int(value) if (-1..=MAX_TELEMETRY_RATE_HZ).contains(&value))
48        {
49            continue;
50        }
51
52        let old = ctx.params.get_by_id(id);
53        if old == req.value {
54            if is_mixer_choice_param(id) {
55                crate::mixer::matrix::sync_reflected_mixer_params(ctx.params, id);
56                emit_reflected_mixer_param_responses(ctx.comm_events, ctx.params, id);
57            }
58            emit_param_value_response(ctx.comm_events, ctx.params.get_by_id(id), id);
59            continue;
60        }
61
62        set_param_and_emit_change(ctx.params, &mut ctx.events.changes, id, req.value);
63        crate::mixer::matrix::sync_reflected_mixer_params(ctx.params, id);
64        let new = ctx.params.get_by_id(id);
65
66        emit_reflected_mixer_param_responses(ctx.comm_events, ctx.params, id);
67        let response = ParamValueMsg {
68            param_id: req.param_id_bytes,
69            param_value: new,
70            param_count: PARAMS_COUNT as u16,
71            param_index: id as u16,
72        };
73        ctx.comm_events
74            .responses
75            .push_or_log(CommResponse::ParamValue(response), "param set response");
76    }
77}
78
79pub fn set_param_and_emit_change(
80    params: &mut Params,
81    changes: &mut EventQueue<ParamChanged, PARAM_CHANGED_QUEUE_CAPACITY>,
82    id: ParamId,
83    value: ParamValue,
84) {
85    let old = params.get_by_id(id);
86    if old == value {
87        return;
88    }
89    params.set_by_id(id, value);
90    let new = params.get_by_id(id);
91    emit_param_change(changes, id, old, new);
92}
93
94/// Queue an onboard parameter-value update after a subsystem has changed a
95/// parameter directly.  Sensor calibration uses this once its sample window
96/// completes, so the companion receives the resulting value as MAVLink
97/// `PARAM_VALUE` rather than only the command acknowledgement.
98pub fn emit_param_change(
99    changes: &mut EventQueue<ParamChanged, PARAM_CHANGED_QUEUE_CAPACITY>,
100    id: ParamId,
101    old: ParamValue,
102    new: ParamValue,
103) {
104    changes.push_or_log(
105        ParamChanged {
106            id,
107            old,
108            new,
109            param_id_bytes: str_to_fixed_bytes(PARAM_DEFINITIONS[id as usize].name),
110        },
111        "param changed event",
112    );
113}
114
115pub fn mark_all_params_changed(events: &mut ParamEventQueues) {
116    events.full_refresh = true;
117}
118
119fn is_mixer_choice_param(id: ParamId) -> bool {
120    matches!(
121        id,
122        ParamId::PARAM_PRIMARY_MIXER | ParamId::PARAM_SECONDARY_MIXER
123    )
124}
125
126fn emit_param_value_response(comm_events: &mut CommEventQueues, value: ParamValue, id: ParamId) {
127    let def = &PARAM_DEFINITIONS[id as usize];
128    let response = ParamValueMsg {
129        param_id: str_to_fixed_bytes(def.name),
130        param_value: value,
131        param_count: PARAMS_COUNT as u16,
132        param_index: id as u16,
133    };
134    comm_events
135        .responses
136        .push_or_log(CommResponse::ParamValue(response), "param value response");
137}
138
139fn emit_reflected_mixer_param_responses(
140    comm_events: &mut CommEventQueues,
141    params: &Params,
142    changed: ParamId,
143) {
144    match changed {
145        ParamId::PARAM_PRIMARY_MIXER => {
146            emit_param_range(
147                comm_events,
148                params,
149                ParamId::PARAM_PRIMARY_MIXER_OUTPUT_0,
150                NUM_MIXER_OUTPUT_PARAMS,
151            );
152            emit_param_range(
153                comm_events,
154                params,
155                ParamId::PARAM_PRIMARY_MIXER_PWM_RATE_0,
156                NUM_MIXER_OUTPUT_PARAMS,
157            );
158            emit_param_range(
159                comm_events,
160                params,
161                ParamId::PARAM_PRIMARY_MIXER_0_0,
162                NUM_MIXER_MATRIX_PARAMS,
163            );
164        }
165        ParamId::PARAM_SECONDARY_MIXER => {
166            emit_param_range(
167                comm_events,
168                params,
169                ParamId::PARAM_SECONDARY_MIXER_0_0,
170                NUM_MIXER_MATRIX_PARAMS,
171            );
172        }
173        _ => {}
174    }
175}
176
177const NUM_MIXER_OUTPUT_PARAMS: usize = 10;
178const NUM_MIXER_MATRIX_PARAMS: usize = 100;
179
180fn emit_param_range(
181    comm_events: &mut CommEventQueues,
182    params: &Params,
183    first_id: ParamId,
184    len: usize,
185) {
186    let first_index = first_id as usize;
187    for offset in 0..len {
188        let Some(id) = ParamId::from_index(first_index + offset) else {
189            return;
190        };
191        emit_param_value_response(comm_events, params.get_by_id(id), id);
192    }
193}
194
195fn same_param_type(lhs: ParamValue, rhs: ParamValue) -> bool {
196    matches!(
197        (lhs, rhs),
198        (ParamValue::Float(_), ParamValue::Float(_))
199            | (ParamValue::Int(_), ParamValue::Int(_))
200            | (ParamValue::Uint(_), ParamValue::Uint(_))
201            | (ParamValue::Bool(_), ParamValue::Bool(_))
202    )
203}
204
205pub fn service_param_read_requests(ctx: &mut ParamServiceCtx<'_>) {
206    while let Some(req) = ctx.events.read_requests.pop() {
207        let Some(id) = param_id_from_identifier(req.identifier) else {
208            continue;
209        };
210        let def = &PARAM_DEFINITIONS[id as usize];
211        let response = ParamValueMsg {
212            param_id: str_to_fixed_bytes(def.name),
213            param_value: ctx.params.get_by_id(id),
214            param_count: PARAMS_COUNT as u16,
215            param_index: id as u16,
216        };
217        ctx.comm_events
218            .responses
219            .push_or_log(CommResponse::ParamValue(response), "param read response");
220    }
221}
222
223pub fn service_param_list_requests(ctx: &mut ParamServiceCtx<'_>) {
224    while ctx.events.list_requests.pop().is_some() {
225        ctx.state.next_index = Some(0);
226    }
227
228    let Some(index) = ctx.state.next_index else {
229        return;
230    };
231
232    let Some(def) = PARAM_DEFINITIONS.get(index) else {
233        ctx.state.next_index = None;
234        return;
235    };
236
237    let response = ParamValueMsg {
238        param_id: str_to_fixed_bytes(def.name),
239        param_value: ctx.params.get_by_id(def.id),
240        param_count: PARAMS_COUNT as u16,
241        param_index: def.id as u16,
242    };
243    ctx.comm_events
244        .responses
245        .push_or_log(CommResponse::ParamValue(response), "param list response");
246
247    let next = index + 1;
248    ctx.state.next_index = (next < PARAMS_COUNT).then_some(next);
249}
250
251fn param_id_from_identifier(identifier: ParamIdentifier) -> Option<ParamId> {
252    match identifier {
253        ParamIdentifier::INDEX(index) if index >= 0 => {
254            PARAM_DEFINITIONS.get(index as usize).map(|def| def.id)
255        }
256        ParamIdentifier::INDEX(_) => None,
257        ParamIdentifier::ID(bytes) => param_id_from_name_bytes(bytes),
258    }
259}
260
261fn param_id_from_name_bytes(bytes: [u8; 16]) -> Option<ParamId> {
262    let len = bytes.iter().position(|&b| b == 0).unwrap_or(bytes.len());
263    let name = core::str::from_utf8(&bytes[..len]).ok()?;
264    PARAM_DEFINITIONS
265        .iter()
266        .find(|def| def.name == name || str_to_fixed_bytes(def.name) == bytes)
267        .map(|def| def.id)
268}
269
270#[cfg(test)]
271mod tests {
272    use super::*;
273    use crate::{
274        events::{
275            CommEventQueues, ParamEventQueues, ParamListRequested, ParamReadRequested,
276            ParamSetRequested,
277        },
278        params::{ParamId, ParamValue, Params},
279    };
280
281    fn test_ctx<'a>(
282        params: &'a mut Params,
283        state: &'a mut ParamListState,
284        events: &'a mut ParamEventQueues,
285        comm_events: &'a mut CommEventQueues,
286    ) -> ParamServiceCtx<'a> {
287        ParamServiceCtx {
288            params,
289            state,
290            events,
291            comm_events,
292        }
293    }
294
295    #[test]
296    fn apply_param_requests_mutates_params_and_defers_ack() {
297        let mut params = Params::new();
298        let mut events = ParamEventQueues::default();
299        let mut comm_events = CommEventQueues::default();
300        let mut state = ParamListState::default();
301
302        let request = ParamSetRequested {
303            value: ParamValue::Int(42),
304            param_id_bytes: *b"SYS_ID\0\0\0\0\0\0\0\0\0\0",
305        };
306        let _ = events.set_requests.push(request);
307
308        apply_param_requests(&mut test_ctx(
309            &mut params,
310            &mut state,
311            &mut events,
312            &mut comm_events,
313        ));
314
315        assert_eq!(
316            params.get_by_id(ParamId::PARAM_SYSTEM_ID),
317            ParamValue::Int(42)
318        );
319
320        let change = events.changes.pop().unwrap();
321        assert_eq!(change.id, ParamId::PARAM_SYSTEM_ID);
322        assert_eq!(change.old, ParamValue::Int(1));
323        assert_eq!(change.new, ParamValue::Int(42));
324
325        match comm_events.responses.pop().unwrap() {
326            CommResponse::ParamValue(response) => {
327                assert_eq!(response.param_index, ParamId::PARAM_SYSTEM_ID as u16);
328                assert_eq!(response.param_value, ParamValue::Int(42));
329            }
330            _ => panic!("expected param value response"),
331        }
332    }
333
334    #[test]
335    fn apply_param_requests_matches_channel_output_mask_name() {
336        let mut params = Params::new();
337        let mut events = ParamEventQueues::default();
338        let mut comm_events = CommEventQueues::default();
339        let mut state = ParamListState::default();
340
341        let request = ParamSetRequested {
342            value: ParamValue::Int(0x0f),
343            param_id_bytes: crate::comm::str_to_fixed_bytes("CHN_OUTPUT_MASK"),
344        };
345        let _ = events.set_requests.push(request);
346
347        apply_param_requests(&mut test_ctx(
348            &mut params,
349            &mut state,
350            &mut events,
351            &mut comm_events,
352        ));
353
354        assert_eq!(
355            params.get_by_id(ParamId::PARAM_CHANNEL_OUTPUT_MASK),
356            ParamValue::Int(0x0f)
357        );
358    }
359
360    #[test]
361    fn apply_param_requests_rejects_out_of_range_telemetry_rate() {
362        let mut params = Params::new();
363        let mut events = ParamEventQueues::default();
364        let mut comm_events = CommEventQueues::default();
365        let mut state = ParamListState::default();
366        let _ = events.set_requests.push(ParamSetRequested {
367            value: ParamValue::Int(MAX_TELEMETRY_RATE_HZ + 1),
368            param_id_bytes: crate::comm::str_to_fixed_bytes("TEL_BARO_HZ"),
369        });
370
371        apply_param_requests(&mut test_ctx(
372            &mut params,
373            &mut state,
374            &mut events,
375            &mut comm_events,
376        ));
377
378        assert_eq!(
379            params.get_by_id(ParamId::PARAM_TELEM_BARO_HZ),
380            ParamValue::Int(100)
381        );
382        assert!(events.changes.is_empty());
383        assert!(comm_events.responses.is_empty());
384    }
385
386    #[test]
387    fn apply_param_requests_ignores_wrong_type_and_acknowledges_unchanged_value() {
388        let mut params = Params::new();
389        let mut events = ParamEventQueues::default();
390        let mut comm_events = CommEventQueues::default();
391        let mut state = ParamListState::default();
392
393        let _ = events.set_requests.push(ParamSetRequested {
394            value: ParamValue::Float(42.0),
395            param_id_bytes: *b"SYS_ID\0\0\0\0\0\0\0\0\0\0",
396        });
397        let _ = events.set_requests.push(ParamSetRequested {
398            value: ParamValue::Int(1),
399            param_id_bytes: *b"SYS_ID\0\0\0\0\0\0\0\0\0\0",
400        });
401
402        apply_param_requests(&mut test_ctx(
403            &mut params,
404            &mut state,
405            &mut events,
406            &mut comm_events,
407        ));
408
409        assert_eq!(
410            params.get_by_id(ParamId::PARAM_SYSTEM_ID),
411            ParamValue::Int(1)
412        );
413        assert!(events.changes.is_empty());
414        match comm_events.responses.pop().unwrap() {
415            CommResponse::ParamValue(response) => {
416                assert_eq!(response.param_index, ParamId::PARAM_SYSTEM_ID as u16);
417                assert_eq!(response.param_value, ParamValue::Int(1));
418            }
419            _ => panic!("expected param value response"),
420        }
421        assert!(comm_events.responses.is_empty());
422    }
423
424    #[test]
425    fn apply_param_requests_refreshes_mixer_reflection_for_unchanged_mixer_choice() {
426        let mut params = Params::new();
427        params.set_by_id(ParamId::PARAM_PRIMARY_MIXER, ParamValue::Int(10));
428        params.set_by_id(ParamId::PARAM_PRIMARY_MIXER_OUTPUT_0, ParamValue::Int(2));
429        let mut events = ParamEventQueues::default();
430        let mut comm_events = CommEventQueues::default();
431        let mut state = ParamListState::default();
432
433        let _ = events.set_requests.push(ParamSetRequested {
434            value: ParamValue::Int(10),
435            param_id_bytes: *b"PRIMARY_MIXER\0\0\0",
436        });
437
438        apply_param_requests(&mut test_ctx(
439            &mut params,
440            &mut state,
441            &mut events,
442            &mut comm_events,
443        ));
444
445        assert_eq!(
446            params.get_by_id(ParamId::PARAM_PRIMARY_MIXER_OUTPUT_0),
447            ParamValue::Int(1)
448        );
449        assert!(events.changes.is_empty());
450        assert!(!comm_events.responses.is_empty());
451    }
452
453    #[test]
454    fn apply_param_requests_emits_mixer_reflection_before_choice_ack() {
455        let mut params = Params::new();
456        let mut events = ParamEventQueues::default();
457        let mut comm_events = CommEventQueues::default();
458        let mut state = ParamListState::default();
459
460        let _ = events.set_requests.push(ParamSetRequested {
461            value: ParamValue::Int(10),
462            param_id_bytes: *b"PRIMARY_MIXER\0\0\0",
463        });
464
465        apply_param_requests(&mut test_ctx(
466            &mut params,
467            &mut state,
468            &mut events,
469            &mut comm_events,
470        ));
471
472        match comm_events.responses.pop().unwrap() {
473            CommResponse::ParamValue(response) => {
474                assert_eq!(
475                    response.param_index,
476                    ParamId::PARAM_PRIMARY_MIXER_OUTPUT_0 as u16
477                );
478                assert_eq!(response.param_value, ParamValue::Int(1));
479            }
480            _ => panic!("expected reflected mixer param response"),
481        }
482
483        for _ in 1..(NUM_MIXER_OUTPUT_PARAMS * 2 + NUM_MIXER_MATRIX_PARAMS) {
484            let _ = comm_events.responses.pop().unwrap();
485        }
486
487        match comm_events.responses.pop().unwrap() {
488            CommResponse::ParamValue(response) => {
489                assert_eq!(response.param_index, ParamId::PARAM_PRIMARY_MIXER as u16);
490                assert_eq!(response.param_value, ParamValue::Int(10));
491            }
492            _ => panic!("expected primary mixer acknowledgement"),
493        }
494    }
495
496    #[test]
497    fn service_param_list_requests_streams_one_param_per_call() {
498        let mut params = Params::new();
499        let mut state = ParamListState::default();
500        let mut events = ParamEventQueues::default();
501        let mut comm_events = CommEventQueues::default();
502
503        let _ = events.list_requests.push(ParamListRequested);
504
505        service_param_list_requests(&mut test_ctx(
506            &mut params,
507            &mut state,
508            &mut events,
509            &mut comm_events,
510        ));
511
512        match comm_events.responses.pop().unwrap() {
513            CommResponse::ParamValue(response) => {
514                assert_eq!(response.param_index, ParamId::PARAM_BAUD_RATE as u16);
515                assert_eq!(response.param_value, ParamValue::Int(921600));
516            }
517            _ => panic!("expected param value response"),
518        }
519        assert!(state.is_active());
520
521        service_param_list_requests(&mut test_ctx(
522            &mut params,
523            &mut state,
524            &mut events,
525            &mut comm_events,
526        ));
527
528        match comm_events.responses.pop().unwrap() {
529            CommResponse::ParamValue(response) => {
530                assert_eq!(response.param_index, ParamId::PARAM_SERIAL_DEVICE as u16);
531            }
532            _ => panic!("expected param value response"),
533        }
534    }
535
536    #[test]
537    fn service_param_read_requests_responds_by_index_and_id() {
538        let mut params = Params::new();
539        params.set_by_id(ParamId::PARAM_SYSTEM_ID, ParamValue::Int(42));
540        let mut events = ParamEventQueues::default();
541        let mut comm_events = CommEventQueues::default();
542        let mut state = ParamListState::default();
543
544        let _ = events.read_requests.push(ParamReadRequested {
545            identifier: ParamIdentifier::INDEX(0),
546        });
547        let _ = events.read_requests.push(ParamReadRequested {
548            identifier: ParamIdentifier::ID(*b"SYS_ID\0\0\0\0\0\0\0\0\0\0"),
549        });
550
551        service_param_read_requests(&mut test_ctx(
552            &mut params,
553            &mut state,
554            &mut events,
555            &mut comm_events,
556        ));
557
558        match comm_events.responses.pop().unwrap() {
559            CommResponse::ParamValue(response) => {
560                assert_eq!(response.param_index, ParamId::PARAM_BAUD_RATE as u16);
561                assert_eq!(response.param_value, ParamValue::Int(921600));
562            }
563            _ => panic!("expected param value response"),
564        }
565
566        match comm_events.responses.pop().unwrap() {
567            CommResponse::ParamValue(response) => {
568                assert_eq!(response.param_index, ParamId::PARAM_SYSTEM_ID as u16);
569                assert_eq!(response.param_value, ParamValue::Int(42));
570            }
571            _ => panic!("expected param value response"),
572        }
573    }
574}
veloxity_core/params/service.rs

veloxity_core/params/
service.rs
