veloxity_core/sensors/
health.rs

1use crate::{
2    math::FlightFloat,
3    params::{ParamId, ParamValue, Params},
4    sensors::ProcessedSensors,
5    state_machine::{ErrorFlag, StateManager},
6};
7
8pub struct SensorHealthCtx<'a, R: FlightFloat> {
9    pub now_us: u64,
10    pub sensors: &'a ProcessedSensors<R>,
11    pub params: &'a Params,
12    pub state: &'a mut StateManager,
13    pub last_imu_seen: &'a mut u64,
14    pub imu_timeout_us: u64,
15}
16
17pub fn update_sensor_health<R: FlightFloat>(ctx: SensorHealthCtx<'_, R>) {
18    if ctx.sensors.imu.is_some() {
19        *ctx.last_imu_seen = ctx.now_us;
20        ctx.state
21            .set_error_flag(ErrorFlag::IMU_NOT_RESPONDING, false, ctx.params);
22    } else if ctx.now_us > *ctx.last_imu_seen + ctx.imu_timeout_us {
23        ctx.state
24            .set_error_flag(ErrorFlag::IMU_NOT_RESPONDING, true, ctx.params);
25    }
26}
27
28/// Establishes ROSflight C's startup IMU-calibration interlock.
29///
30/// This must only be evaluated during initialization. Once an all-zero bias
31/// table latches the error, live parameter changes and gyro-only calibration
32/// must not clear it. ROSflight C clears the latch only when a full
33/// accelerometer/IMU calibration succeeds.
34pub(crate) fn initialize_imu_calibration_error(state: &mut StateManager, params: &Params) {
35    let error = ErrorFlag::UNCALIBRATED_IMU;
36    state.set_error_flag(error, imu_bias_params_are_all_zero(params), params);
37}
38
39fn imu_bias_params_are_all_zero(params: &Params) -> bool {
40    [
41        ParamId::PARAM_ACC_X_BIAS,
42        ParamId::PARAM_ACC_Y_BIAS,
43        ParamId::PARAM_ACC_Z_BIAS,
44        ParamId::PARAM_GYRO_X_BIAS,
45        ParamId::PARAM_GYRO_Y_BIAS,
46        ParamId::PARAM_GYRO_Z_BIAS,
47    ]
48    .into_iter()
49    .all(|id| matches!(params.get_by_id(id), ParamValue::Float(value) if value == 0.0))
50}
51
52#[cfg(test)]
53mod tests {
54    use super::*;
55    use crate::{
56        packets::{ImuPacket, RosflightPacketHeader},
57        state_machine::Event,
58    };
59
60    const TEST_IMU_TIMEOUT_US: u64 = 100_000;
61
62    fn imu_packet(timestamp: u64) -> ImuPacket<f64> {
63        ImuPacket {
64            header: RosflightPacketHeader {
65                timestamp,
66                status: 0,
67            },
68            accel: [0.0, 0.0, -9.80665],
69            gyro: [0.0, 0.0, 0.0],
70            temperature: 25.0,
71            seq: 1,
72        }
73    }
74
75    #[test]
76    fn imu_calibration_startup_latch_survives_sensor_health_updates() {
77        let params = Params::new();
78        let mut state = StateManager::new();
79        state.update(Event::INITIALIZED, &params);
80        initialize_imu_calibration_error(&mut state, &params);
81        let mut last_imu_seen = 0;
82        let sensors = ProcessedSensors {
83            imu: Some(imu_packet(1)),
84            ..ProcessedSensors::<f64>::default()
85        };
86
87        update_sensor_health(SensorHealthCtx {
88            now_us: 1,
89            sensors: &sensors,
90            params: &params,
91            state: &mut state,
92            last_imu_seen: &mut last_imu_seen,
93            imu_timeout_us: TEST_IMU_TIMEOUT_US,
94        });
95
96        assert!(state.get_errors().contains(ErrorFlag::UNCALIBRATED_IMU));
97        assert_eq!(last_imu_seen, 1);
98    }
99
100    #[test]
101    fn imu_calibration_health_does_not_clear_latched_error_after_live_bias_change() {
102        let mut params = Params::new();
103        params.set_by_id(ParamId::PARAM_ACC_X_BIAS, ParamValue::Float(0.01));
104        let mut state = StateManager::new();
105        state.update(Event::INITIALIZED, &params);
106        state.update(Event::ERROR_OCCURRED(ErrorFlag::UNCALIBRATED_IMU), &params);
107        let mut last_imu_seen = 0;
108        let sensors = ProcessedSensors {
109            imu: Some(imu_packet(1)),
110            ..ProcessedSensors::<f64>::default()
111        };
112
113        update_sensor_health(SensorHealthCtx {
114            now_us: 1,
115            sensors: &sensors,
116            params: &params,
117            state: &mut state,
118            last_imu_seen: &mut last_imu_seen,
119            imu_timeout_us: TEST_IMU_TIMEOUT_US,
120        });
121
122        assert!(state.get_errors().contains(ErrorFlag::UNCALIBRATED_IMU));
123    }
124
125    #[test]
126    fn sensor_health_sets_imu_not_responding_after_timeout_without_imu_sample() {
127        let params = Params::new();
128        let mut state = StateManager::new();
129        state.update(Event::INITIALIZED, &params);
130        let mut last_imu_seen = 0;
131        let sensors = ProcessedSensors::<f64>::default();
132
133        update_sensor_health(SensorHealthCtx {
134            now_us: TEST_IMU_TIMEOUT_US + 1,
135            sensors: &sensors,
136            params: &params,
137            state: &mut state,
138            last_imu_seen: &mut last_imu_seen,
139            imu_timeout_us: TEST_IMU_TIMEOUT_US,
140        });
141
142        assert!(state.get_errors().contains(ErrorFlag::IMU_NOT_RESPONDING));
143        assert_eq!(last_imu_seen, 0);
144    }
145
146    #[test]
147    fn sensor_health_clears_imu_not_responding_when_imu_sample_returns() {
148        let params = Params::new();
149        let mut state = StateManager::new();
150        state.update(Event::INITIALIZED, &params);
151        state.update(
152            Event::ERROR_OCCURRED(ErrorFlag::IMU_NOT_RESPONDING),
153            &params,
154        );
155        let mut last_imu_seen = 0;
156        let sensors = ProcessedSensors {
157            imu: Some(imu_packet(5)),
158            ..ProcessedSensors::<f64>::default()
159        };
160
161        update_sensor_health(SensorHealthCtx {
162            now_us: 5,
163            sensors: &sensors,
164            params: &params,
165            state: &mut state,
166            last_imu_seen: &mut last_imu_seen,
167            imu_timeout_us: TEST_IMU_TIMEOUT_US,
168        });
169
170        assert!(!state.get_errors().contains(ErrorFlag::IMU_NOT_RESPONDING));
171        assert_eq!(last_imu_seen, 5);
172    }
173}
veloxity_core/sensors/health.rs

veloxity_core/sensors/
health.rs
