ados_protocol/
param_codec.rs

1//! The value encoding of MAVLink `PARAM_SET` and `PARAM_VALUE`.
2//!
3//! Both messages carry the value in one 4-byte float field. ArduPilot casts
4//! every value to float, integer parameters included. PX4 instead copies an
5//! integer parameter's bytes into the field (bytewise encoding), so the float
6//! it sends for the INT32 value `5` is the bit pattern of the integer 5, not
7//! `5.0`. Reading or writing a PX4 integer parameter as a float corrupts it:
8//! `5` written as a float lands as `1084227584`. The router's decode and the
9//! control surface's encode share this one codec so the two never disagree.
10
11use crate::flight_modes::MAV_AUTOPILOT_PX4;
12
13/// The `MAV_PARAM_TYPE` values the codec distinguishes.
14pub const MAV_PARAM_TYPE_UINT8: u8 = 1;
15pub const MAV_PARAM_TYPE_INT8: u8 = 2;
16pub const MAV_PARAM_TYPE_UINT16: u8 = 3;
17pub const MAV_PARAM_TYPE_INT16: u8 = 4;
18pub const MAV_PARAM_TYPE_UINT32: u8 = 5;
19pub const MAV_PARAM_TYPE_INT32: u8 = 6;
20pub const MAV_PARAM_TYPE_UINT64: u8 = 7;
21pub const MAV_PARAM_TYPE_INT64: u8 = 8;
22pub const MAV_PARAM_TYPE_REAL32: u8 = 9;
23pub const MAV_PARAM_TYPE_REAL64: u8 = 10;
24
25/// Whether a vehicle whose HEARTBEAT names `autopilot` packs integer
26/// parameters bytewise. PX4 does; ArduPilot casts.
27#[must_use]
28pub fn uses_bytewise(autopilot: i64) -> bool {
29    autopilot == MAV_AUTOPILOT_PX4
30}
31
32/// Whether `param_type` is a defined `MAV_PARAM_TYPE` (UINT8 through REAL64).
33#[must_use]
34pub fn is_known_type(param_type: u8) -> bool {
35    (MAV_PARAM_TYPE_UINT8..=MAV_PARAM_TYPE_REAL64).contains(&param_type)
36}
37
38/// The inclusive range an integer `param_type` can hold, or `None` for a float
39/// type. The 64-bit types are bounded at 2^53, the largest integer the request's
40/// JSON number carries exactly.
41fn integer_range(param_type: u8) -> Option<(f64, f64)> {
42    const EXACT: f64 = 9_007_199_254_740_992.0;
43    Some(match param_type {
44        MAV_PARAM_TYPE_UINT8 => (0.0, f64::from(u8::MAX)),
45        MAV_PARAM_TYPE_INT8 => (f64::from(i8::MIN), f64::from(i8::MAX)),
46        MAV_PARAM_TYPE_UINT16 => (0.0, f64::from(u16::MAX)),
47        MAV_PARAM_TYPE_INT16 => (f64::from(i16::MIN), f64::from(i16::MAX)),
48        MAV_PARAM_TYPE_UINT32 => (0.0, f64::from(u32::MAX)),
49        MAV_PARAM_TYPE_INT32 => (f64::from(i32::MIN), f64::from(i32::MAX)),
50        MAV_PARAM_TYPE_UINT64 => (0.0, EXACT),
51        MAV_PARAM_TYPE_INT64 => (-EXACT, EXACT),
52        _ => return None,
53    })
54}
55
56/// The value a `PARAM_VALUE` reported, decoded from its wire field. With
57/// `bytewise`, UINT8 through INT32 are read from the field's low bytes as that
58/// integer type; every other case is the float itself.
59#[must_use]
60pub fn decode(raw: f32, param_type: u8, bytewise: bool) -> f64 {
61    if bytewise {
62        let b = raw.to_le_bytes();
63        match param_type {
64            MAV_PARAM_TYPE_UINT8 => return f64::from(b[0]),
65            MAV_PARAM_TYPE_INT8 => return f64::from(i8::from_le_bytes([b[0]])),
66            MAV_PARAM_TYPE_UINT16 => return f64::from(u16::from_le_bytes([b[0], b[1]])),
67            MAV_PARAM_TYPE_INT16 => return f64::from(i16::from_le_bytes([b[0], b[1]])),
68            MAV_PARAM_TYPE_UINT32 => return f64::from(u32::from_le_bytes(b)),
69            MAV_PARAM_TYPE_INT32 => return f64::from(i32::from_le_bytes(b)),
70            _ => {}
71        }
72    }
73    f64::from(raw)
74}
75
76/// A value ready for a `PARAM_SET`: the float field to send, and the value the
77/// vehicle will hold once it applies it (what its `PARAM_VALUE` echo decodes to).
78#[derive(Debug, Clone, Copy, PartialEq)]
79pub struct Encoded {
80    pub wire: f32,
81    pub stored: f64,
82}
83
84/// Encode `value` for a parameter of `param_type`. An integer type takes the
85/// rounded value and refuses one outside the type's range; with `bytewise`,
86/// UINT8 through INT32 are written into the field's low bytes (the rest zero).
87/// A float type refuses a value that overflows a 32-bit float.
88pub fn encode(value: f64, param_type: u8, bytewise: bool) -> Result<Encoded, String> {
89    if !value.is_finite() {
90        return Err("value must be a finite number".to_string());
91    }
92    if let Some((min, max)) = integer_range(param_type) {
93        let n = value.round();
94        if n < min || n > max {
95            return Err(format!(
96                "value {value} is outside the range of this parameter's type ({min} to {max})"
97            ));
98        }
99        if bytewise && param_type <= MAV_PARAM_TYPE_INT32 {
100            let mut b = [0u8; 4];
101            match param_type {
102                MAV_PARAM_TYPE_UINT8 => b[0] = n as u8,
103                MAV_PARAM_TYPE_INT8 => b[0] = (n as i8).to_le_bytes()[0],
104                MAV_PARAM_TYPE_UINT16 => b[..2].copy_from_slice(&(n as u16).to_le_bytes()),
105                MAV_PARAM_TYPE_INT16 => b[..2].copy_from_slice(&(n as i16).to_le_bytes()),
106                MAV_PARAM_TYPE_UINT32 => b = (n as u32).to_le_bytes(),
107                _ => b = (n as i32).to_le_bytes(),
108            }
109            return Ok(Encoded {
110                wire: f32::from_le_bytes(b),
111                stored: n,
112            });
113        }
114        // A casting firmware stores the float it was sent, converted back to
115        // an integer, so the echo is that float.
116        let wire = n as f32;
117        return Ok(Encoded {
118            wire,
119            stored: f64::from(wire),
120        });
121    }
122    let wire = value as f32;
123    if !wire.is_finite() {
124        return Err(format!("value {value} does not fit a 32-bit float"));
125    }
126    Ok(Encoded {
127        wire,
128        stored: f64::from(wire),
129    })
130}
131
132/// Whether a cached echo matches the value a write expects the vehicle to hold.
133/// The echo travelled as a 32-bit float, so equality is judged at that
134/// precision rather than at f64's.
135#[must_use]
136pub fn echo_matches(cached: f64, stored: f64) -> bool {
137    (cached - stored).abs() <= stored.abs().max(1.0) * f64::from(f32::EPSILON)
138}
139
140#[cfg(test)]
141mod tests {
142    use super::*;
143
144    #[test]
145    fn a_px4_integer_travels_as_its_bytes_not_as_a_float() {
146        let e = encode(5.0, MAV_PARAM_TYPE_INT32, true).unwrap();
147        assert_eq!(e.wire.to_le_bytes(), 5i32.to_le_bytes());
148        assert_eq!(e.stored, 5.0);
149        assert_eq!(decode(e.wire, MAV_PARAM_TYPE_INT32, true), 5.0);
150        // The float reading of those bytes is the corruption the codec prevents.
151        assert_ne!(f64::from(e.wire), 5.0);
152    }
153
154    #[test]
155    fn narrow_and_signed_px4_integers_fill_only_their_low_bytes() {
156        let e = encode(-1.0, MAV_PARAM_TYPE_INT8, true).unwrap();
157        assert_eq!(e.wire.to_le_bytes(), [0xFF, 0, 0, 0]);
158        assert_eq!(decode(e.wire, MAV_PARAM_TYPE_INT8, true), -1.0);
159        let e = encode(-300.0, MAV_PARAM_TYPE_INT16, true).unwrap();
160        assert_eq!(&e.wire.to_le_bytes()[..2], &(-300i16).to_le_bytes());
161        assert_eq!(&e.wire.to_le_bytes()[2..], &[0, 0]);
162        assert_eq!(decode(e.wire, MAV_PARAM_TYPE_INT16, true), -300.0);
163        let e = encode(4_000_000_000.0, MAV_PARAM_TYPE_UINT32, true).unwrap();
164        assert_eq!(decode(e.wire, MAV_PARAM_TYPE_UINT32, true), 4_000_000_000.0);
165    }
166
167    #[test]
168    fn an_ardupilot_integer_is_the_float_of_the_rounded_value() {
169        let e = encode(4.6, MAV_PARAM_TYPE_INT32, false).unwrap();
170        assert_eq!(e.wire, 5.0);
171        assert_eq!(e.stored, 5.0);
172        assert_eq!(decode(e.wire, MAV_PARAM_TYPE_INT32, false), 5.0);
173    }
174
175    #[test]
176    fn a_float_param_is_the_same_on_either_firmware() {
177        for bytewise in [false, true] {
178            let e = encode(0.135, MAV_PARAM_TYPE_REAL32, bytewise).unwrap();
179            assert_eq!(e.wire, 0.135f32);
180            assert_eq!(
181                decode(e.wire, MAV_PARAM_TYPE_REAL32, bytewise),
182                f64::from(0.135f32)
183            );
184        }
185    }
186
187    #[test]
188    fn a_value_outside_the_type_is_refused_not_wrapped() {
189        assert!(encode(256.0, MAV_PARAM_TYPE_UINT8, true).is_err());
190        assert!(encode(-1.0, MAV_PARAM_TYPE_UINT8, false).is_err());
191        assert!(encode(128.0, MAV_PARAM_TYPE_INT8, true).is_err());
192        assert!(encode(2_147_483_648.0, MAV_PARAM_TYPE_INT32, true).is_err());
193        assert!(encode(1e39, MAV_PARAM_TYPE_REAL32, false).is_err());
194        assert!(encode(f64::NAN, MAV_PARAM_TYPE_REAL32, false).is_err());
195        // The range edges themselves are writable.
196        assert!(encode(255.0, MAV_PARAM_TYPE_UINT8, true).is_ok());
197        assert!(encode(-128.0, MAV_PARAM_TYPE_INT8, true).is_ok());
198    }
199
200    #[test]
201    fn a_landed_float_write_matches_its_echo_at_f32_precision() {
202        // 100.1 is 100.09999847 as an f32: an f64 comparison at 1e-6 calls that
203        // a miss although the vehicle holds exactly what was sent.
204        let e = encode(100.1, MAV_PARAM_TYPE_REAL32, false).unwrap();
205        assert!(echo_matches(f64::from(100.1f32), e.stored));
206        assert!(!echo_matches(100.2, e.stored));
207        assert!(echo_matches(
208            0.0,
209            encode(0.0, MAV_PARAM_TYPE_REAL32, false).unwrap().stored
210        ));
211        assert!(!echo_matches(1.0, 2.0));
212    }
213
214    #[test]
215    fn only_px4_packs_bytewise() {
216        assert!(uses_bytewise(12));
217        assert!(!uses_bytewise(3));
218        assert!(!uses_bytewise(0));
219        assert!(is_known_type(6) && is_known_type(9) && !is_known_type(0) && !is_known_type(11));
220    }
221}
ados_protocol/param_codec.rs

ados_protocol/
param_codec.rs
