veloxity_core/params/
storage.rs

1//! ROSflight 2.0-compatible nonvolatile parameter image codec.
2
3use super::{PARAM_DEFINITIONS, PARAMS_COUNT, ParamDefinition, ParamId, ParamValue, Params};
4
5include!(concat!(env!("OUT_DIR"), "/veloxity_version.rs"));
6
7/// Byte size of ROSflight 2.0's ARM C++ `params_t`.
8pub const ROSFLIGHT_C_PARAM_STORAGE_SIZE: usize = 7004;
9
10const ROSFLIGHT_C_PARAM_COUNT: usize = 333;
11const ROSFLIGHT_C_PARAM_NAME_SIZE: usize = 16;
12const PARAM_STORAGE_VALUE_SIZE: usize = size_of::<u32>();
13// ARM's C++ ABI stores ROSflight's three-variant param_type_t in one byte.
14const PARAM_STORAGE_TYPE_SIZE: usize = size_of::<u8>();
15const PARAM_STORAGE_VALUES_OFFSET: usize = 8;
16const PARAM_STORAGE_NAMES_OFFSET: usize =
17    PARAM_STORAGE_VALUES_OFFSET + ROSFLIGHT_C_PARAM_COUNT * PARAM_STORAGE_VALUE_SIZE;
18const PARAM_STORAGE_TYPES_OFFSET: usize =
19    PARAM_STORAGE_NAMES_OFFSET + ROSFLIGHT_C_PARAM_COUNT * ROSFLIGHT_C_PARAM_NAME_SIZE;
20const PARAM_STORAGE_MAGIC_EF_OFFSET: usize =
21    PARAM_STORAGE_TYPES_OFFSET + ROSFLIGHT_C_PARAM_COUNT * PARAM_STORAGE_TYPE_SIZE;
22const PARAM_STORAGE_CHECKSUM_OFFSET: usize = PARAM_STORAGE_MAGIC_EF_OFFSET + 1;
23const RUST_ONLY_PARAM_COUNT: usize = 16;
24const ROSFLIGHT_C_TAIL_ORDER: [ParamId; 5] = [
25    ParamId::PARAM_OFFBOARD_TIMEOUT,
26    ParamId::PARAM_BATTERY_VOLTAGE_MULTIPLIER,
27    ParamId::PARAM_BATTERY_CURRENT_MULTIPLIER,
28    ParamId::PARAM_BATTERY_VOLTAGE_ALPHA,
29    ParamId::PARAM_BATTERY_CURRENT_ALPHA,
30];
31
32const _: () = assert!(PARAMS_COUNT - RUST_ONLY_PARAM_COUNT == ROSFLIGHT_C_PARAM_COUNT);
33const _: () = assert!(PARAM_STORAGE_CHECKSUM_OFFSET < ROSFLIGHT_C_PARAM_STORAGE_SIZE);
34
35fn is_rosflight_c_param(id: ParamId) -> bool {
36    !matches!(
37        id,
38        ParamId::PARAM_CHANNEL_OUTPUT_MASK
39            | ParamId::PARAM_ALLOW_UNHEALTHY_ESTIMATOR
40            | ParamId::PARAM_EST_ANGLE_LOCKOUT
41            | ParamId::PARAM_RC_OUTPUT_KILL_CHANNEL
42            | ParamId::PARAM_TELEM_HEARTBEAT_HZ
43            | ParamId::PARAM_TELEM_STATUS_HZ
44            | ParamId::PARAM_TELEM_IMU_HZ
45            | ParamId::PARAM_TELEM_ATTITUDE_HZ
46            | ParamId::PARAM_TELEM_OUTPUT_RAW_HZ
47            | ParamId::PARAM_TELEM_DIFF_PRESSURE_HZ
48            | ParamId::PARAM_TELEM_BARO_HZ
49            | ParamId::PARAM_TELEM_MAG_HZ
50            | ParamId::PARAM_TELEM_RANGE_HZ
51            | ParamId::PARAM_TELEM_BATTERY_HZ
52            | ParamId::PARAM_TELEM_GNSS_HZ
53            | ParamId::PARAM_TELEM_RC_HZ
54    )
55}
56
57fn rosflight_c_param_type(value: ParamValue) -> Option<u8> {
58    match value {
59        ParamValue::Int(_) => Some(0),
60        ParamValue::Float(_) => Some(1),
61        ParamValue::Uint(_) | ParamValue::Bool(_) => None,
62    }
63}
64
65fn is_rosflight_c_tail_param(id: ParamId) -> bool {
66    ROSFLIGHT_C_TAIL_ORDER.contains(&id)
67}
68
69/// Iterates parameters in the exact order of ROSflight C's `ParamId` enum.
70///
71/// Rust declares the battery parameters before `OFFBOARD_TIMEOUT`, matching
72/// C's default-initialization order but not its persisted enum layout. Keep
73/// the common prefix in declaration order and append the divergent C tail
74/// explicitly so names, types, and values occupy the IDs C uses at runtime.
75fn rosflight_c_params() -> impl Iterator<Item = &'static ParamDefinition> {
76    PARAM_DEFINITIONS
77        .iter()
78        .filter(|definition| {
79            is_rosflight_c_param(definition.id) && !is_rosflight_c_tail_param(definition.id)
80        })
81        .chain(
82            ROSFLIGHT_C_TAIL_ORDER
83                .iter()
84                .map(|id| &PARAM_DEFINITIONS[*id as usize]),
85        )
86}
87
88fn rosflight_c_checksum(bytes: &[u8; ROSFLIGHT_C_PARAM_STORAGE_SIZE]) -> u8 {
89    bytes[PARAM_STORAGE_VALUES_OFFSET..PARAM_STORAGE_MAGIC_EF_OFFSET]
90        .iter()
91        .fold(0, |checksum, byte| checksum ^ byte)
92}
93
94fn encode_param_value(value: ParamValue, definition: &ParamDefinition) -> Option<[u8; 4]> {
95    match (value, definition.default) {
96        (ParamValue::Float(value), ParamValue::Float(_)) => Some(value.to_bits().to_le_bytes()),
97        (ParamValue::Int(value), ParamValue::Int(_)) => Some(value.to_le_bytes()),
98        _ => None,
99    }
100}
101
102fn decode_param_value(bytes: [u8; 4], definition: &ParamDefinition) -> Option<ParamValue> {
103    Some(match definition.default {
104        ParamValue::Float(_) => ParamValue::Float(f32::from_bits(u32::from_le_bytes(bytes))),
105        ParamValue::Int(_) => ParamValue::Int(i32::from_le_bytes(bytes)),
106        ParamValue::Uint(_) | ParamValue::Bool(_) => return None,
107    })
108}
109
110/// Encodes the C-compatible subset into ROSflight 2.0's persisted `params_t`.
111pub fn encode_rosflight_c_params(
112    params: &Params,
113    bytes: &mut [u8; ROSFLIGHT_C_PARAM_STORAGE_SIZE],
114) -> bool {
115    bytes.fill(0);
116    bytes[..4].copy_from_slice(&VELOXITY_VERSION.to_le_bytes());
117    bytes[4..6].copy_from_slice(&(ROSFLIGHT_C_PARAM_STORAGE_SIZE as u16).to_le_bytes());
118    bytes[6] = 0xBE;
119
120    let mut count = 0;
121    for (index, definition) in rosflight_c_params().enumerate() {
122        count += 1;
123        let offset = PARAM_STORAGE_VALUES_OFFSET + index * PARAM_STORAGE_VALUE_SIZE;
124        let Some(encoded) = encode_param_value(params.get_by_id(definition.id), definition) else {
125            return false;
126        };
127        bytes[offset..offset + PARAM_STORAGE_VALUE_SIZE].copy_from_slice(&encoded);
128
129        let name_offset = PARAM_STORAGE_NAMES_OFFSET + index * ROSFLIGHT_C_PARAM_NAME_SIZE;
130        let name = definition.name.as_bytes();
131        if name.len() > ROSFLIGHT_C_PARAM_NAME_SIZE {
132            return false;
133        }
134        bytes[name_offset..name_offset + name.len()].copy_from_slice(name);
135
136        let type_offset = PARAM_STORAGE_TYPES_OFFSET + index * PARAM_STORAGE_TYPE_SIZE;
137        let Some(param_type) = rosflight_c_param_type(definition.default) else {
138            return false;
139        };
140        bytes[type_offset] = param_type;
141    }
142    if count != ROSFLIGHT_C_PARAM_COUNT {
143        return false;
144    }
145
146    bytes[PARAM_STORAGE_MAGIC_EF_OFFSET] = 0xEF;
147    bytes[PARAM_STORAGE_CHECKSUM_OFFSET] = rosflight_c_checksum(bytes);
148    true
149}
150
151fn decode_rosflight_c_params_in_order(
152    bytes: &[u8; ROSFLIGHT_C_PARAM_STORAGE_SIZE],
153    definitions: impl Iterator<Item = &'static ParamDefinition>,
154) -> Option<Params> {
155    let mut params = Params::default();
156    let mut count = 0;
157    for (index, definition) in definitions.enumerate() {
158        count += 1;
159        let name_offset = PARAM_STORAGE_NAMES_OFFSET + index * ROSFLIGHT_C_PARAM_NAME_SIZE;
160        let stored_name = &bytes[name_offset..name_offset + ROSFLIGHT_C_PARAM_NAME_SIZE];
161        let expected_name = definition.name.as_bytes();
162        if stored_name[..expected_name.len()] != *expected_name
163            || (expected_name.len() < ROSFLIGHT_C_PARAM_NAME_SIZE
164                && stored_name[expected_name.len()] != 0)
165        {
166            return None;
167        }
168
169        let type_offset = PARAM_STORAGE_TYPES_OFFSET + index * PARAM_STORAGE_TYPE_SIZE;
170        if bytes[type_offset] != rosflight_c_param_type(definition.default)? {
171            return None;
172        }
173
174        let offset = PARAM_STORAGE_VALUES_OFFSET + index * PARAM_STORAGE_VALUE_SIZE;
175        let value = decode_param_value(
176            bytes[offset..offset + PARAM_STORAGE_VALUE_SIZE]
177                .try_into()
178                .ok()?,
179            definition,
180        )?;
181        params.set_by_id(definition.id, value);
182    }
183
184    (count == ROSFLIGHT_C_PARAM_COUNT).then_some(params)
185}
186
187/// Decodes ROSflight 2.0's persisted `params_t` into the Rust parameter table.
188/// Rust-only parameters retain their defaults because they are absent on disk.
189pub fn decode_rosflight_c_params(bytes: &[u8; ROSFLIGHT_C_PARAM_STORAGE_SIZE]) -> Option<Params> {
190    let version = u32::from_le_bytes(bytes[..4].try_into().ok()?);
191    if version != VELOXITY_VERSION
192        || usize::from(u16::from_le_bytes(bytes[4..6].try_into().ok()?))
193            != ROSFLIGHT_C_PARAM_STORAGE_SIZE
194        || bytes[6] != 0xBE
195        || bytes[PARAM_STORAGE_MAGIC_EF_OFFSET] != 0xEF
196        || bytes[PARAM_STORAGE_CHECKSUM_OFFSET] != rosflight_c_checksum(bytes)
197    {
198        return None;
199    }
200
201    decode_rosflight_c_params_in_order(bytes, rosflight_c_params())
202}
203
204#[cfg(test)]
205mod tests {
206    use super::*;
207
208    fn stored_name(bytes: &[u8; ROSFLIGHT_C_PARAM_STORAGE_SIZE], index: usize) -> &str {
209        let offset = PARAM_STORAGE_NAMES_OFFSET + index * ROSFLIGHT_C_PARAM_NAME_SIZE;
210        let field = &bytes[offset..offset + ROSFLIGHT_C_PARAM_NAME_SIZE];
211        let length = field
212            .iter()
213            .position(|byte| *byte == 0)
214            .unwrap_or(field.len());
215        core::str::from_utf8(&field[..length]).unwrap()
216    }
217
218    #[test]
219    fn storage_format_round_trips_only_c_parameters() {
220        let mut params = Params::default();
221        params.set_by_id(ParamId::PARAM_SYSTEM_ID, ParamValue::Int(42));
222        params.set_by_id(ParamId::PARAM_GYRO_X_BIAS, ParamValue::Float(-0.25));
223        params.set_by_id(ParamId::PARAM_CHANNEL_OUTPUT_MASK, ParamValue::Int(0));
224        params.set_by_id(ParamId::PARAM_ALLOW_UNHEALTHY_ESTIMATOR, ParamValue::Int(0));
225        params.set_by_id(ParamId::PARAM_EST_ANGLE_LOCKOUT, ParamValue::Int(1));
226        params.set_by_id(ParamId::PARAM_RC_OUTPUT_KILL_CHANNEL, ParamValue::Int(4));
227        params.set_by_id(ParamId::PARAM_TELEM_BARO_HZ, ParamValue::Int(20));
228
229        let mut bytes = [0; ROSFLIGHT_C_PARAM_STORAGE_SIZE];
230        assert!(encode_rosflight_c_params(&params, &mut bytes));
231        let decoded = decode_rosflight_c_params(&bytes).expect("valid image must decode");
232
233        assert_eq!(
234            decoded.get_by_id(ParamId::PARAM_SYSTEM_ID),
235            ParamValue::Int(42)
236        );
237        assert_eq!(
238            decoded.get_by_id(ParamId::PARAM_GYRO_X_BIAS),
239            ParamValue::Float(-0.25)
240        );
241        assert_eq!(
242            decoded.get_by_id(ParamId::PARAM_CHANNEL_OUTPUT_MASK),
243            ParamValue::Int(0x0f)
244        );
245        assert_eq!(
246            decoded.get_by_id(ParamId::PARAM_ALLOW_UNHEALTHY_ESTIMATOR),
247            ParamValue::Int(1)
248        );
249        assert_eq!(
250            decoded.get_by_id(ParamId::PARAM_EST_ANGLE_LOCKOUT),
251            ParamValue::Int(0)
252        );
253        assert_eq!(
254            decoded.get_by_id(ParamId::PARAM_RC_OUTPUT_KILL_CHANNEL),
255            ParamValue::Int(-1)
256        );
257        assert_eq!(
258            decoded.get_by_id(ParamId::PARAM_TELEM_BARO_HZ),
259            ParamValue::Int(0)
260        );
261    }
262
263    #[test]
264    fn storage_matches_rosflight_c_arm_layout() {
265        let mut bytes = [0; ROSFLIGHT_C_PARAM_STORAGE_SIZE];
266        assert!(encode_rosflight_c_params(&Params::default(), &mut bytes));
267
268        assert_eq!(&bytes[..4], &VELOXITY_VERSION.to_le_bytes());
269        assert_eq!(
270            &bytes[4..6],
271            &(ROSFLIGHT_C_PARAM_STORAGE_SIZE as u16).to_le_bytes()
272        );
273        assert_eq!(bytes[6], 0xBE);
274        assert_eq!(bytes[7], 0);
275        assert_eq!(bytes[PARAM_STORAGE_MAGIC_EF_OFFSET], 0xEF);
276        assert_eq!(
277            bytes[PARAM_STORAGE_CHECKSUM_OFFSET],
278            rosflight_c_checksum(&bytes)
279        );
280        assert_eq!(
281            &bytes[PARAM_STORAGE_NAMES_OFFSET..PARAM_STORAGE_NAMES_OFFSET + 10],
282            b"BAUD_RATE\0"
283        );
284        assert_eq!(bytes[PARAM_STORAGE_TYPES_OFFSET], 0);
285    }
286
287    #[test]
288    fn storage_tail_matches_rosflight_c_param_ids() {
289        let mut bytes = [0; ROSFLIGHT_C_PARAM_STORAGE_SIZE];
290        assert!(encode_rosflight_c_params(&Params::default(), &mut bytes));
291
292        let expected = [
293            (328, "OFFBOARD_TIMEOUT", 0),
294            (329, "BATT_VOLT_MULT", 1),
295            (330, "BATT_CURR_MULT", 1),
296            (331, "BATT_VOLT_LPF", 1),
297            (332, "BATT_CURR_LPF", 1),
298        ];
299        for (index, name, param_type) in expected {
300            assert_eq!(stored_name(&bytes, index), name);
301            assert_eq!(bytes[PARAM_STORAGE_TYPES_OFFSET + index], param_type);
302        }
303    }
304
305    #[test]
306    fn storage_rejects_misordered_tail() {
307        let mut bytes = [0; ROSFLIGHT_C_PARAM_STORAGE_SIZE];
308        assert!(encode_rosflight_c_params(&Params::default(), &mut bytes));
309
310        let first = PARAM_STORAGE_NAMES_OFFSET + 328 * ROSFLIGHT_C_PARAM_NAME_SIZE;
311        let second = PARAM_STORAGE_NAMES_OFFSET + 329 * ROSFLIGHT_C_PARAM_NAME_SIZE;
312        let mut first_name = [0; ROSFLIGHT_C_PARAM_NAME_SIZE];
313        let mut second_name = [0; ROSFLIGHT_C_PARAM_NAME_SIZE];
314        first_name.copy_from_slice(&bytes[first..first + ROSFLIGHT_C_PARAM_NAME_SIZE]);
315        second_name.copy_from_slice(&bytes[second..second + ROSFLIGHT_C_PARAM_NAME_SIZE]);
316        bytes[first..first + ROSFLIGHT_C_PARAM_NAME_SIZE].copy_from_slice(&second_name);
317        bytes[second..second + ROSFLIGHT_C_PARAM_NAME_SIZE].copy_from_slice(&first_name);
318        bytes[PARAM_STORAGE_CHECKSUM_OFFSET] = rosflight_c_checksum(&bytes);
319
320        assert!(decode_rosflight_c_params(&bytes).is_none());
321    }
322
323    #[test]
324    fn storage_rejects_version_mismatch() {
325        let mut bytes = [0; ROSFLIGHT_C_PARAM_STORAGE_SIZE];
326        assert!(encode_rosflight_c_params(&Params::default(), &mut bytes));
327        bytes[0] ^= 1;
328        assert!(decode_rosflight_c_params(&bytes).is_none());
329    }
330
331    #[test]
332    fn storage_rejects_schema_mismatch_even_with_valid_checksum() {
333        let mut bytes = [0; ROSFLIGHT_C_PARAM_STORAGE_SIZE];
334        assert!(encode_rosflight_c_params(&Params::default(), &mut bytes));
335        bytes[PARAM_STORAGE_NAMES_OFFSET] ^= 1;
336        bytes[PARAM_STORAGE_CHECKSUM_OFFSET] = rosflight_c_checksum(&bytes);
337        assert!(decode_rosflight_c_params(&bytes).is_none());
338    }
339}
veloxity_core/params/storage.rs

veloxity_core/params/
storage.rs
