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        update_imu_calibration_error(ctx.state, ctx.params);
23    } else if ctx.now_us > *ctx.last_imu_seen + ctx.imu_timeout_us {
24        ctx.state
25            .set_error_flag(ErrorFlag::IMU_NOT_RESPONDING, true, ctx.params);
26    }
27}
28
29fn update_imu_calibration_error(state: &mut StateManager, params: &Params) {
30    let error = ErrorFlag::UNCALIBRATED_IMU;
31    if imu_bias_params_are_all_zero(params) {
32        state.set_error_flag(error, true, params);
33    } else {
34        state.set_error_flag(error, false, params);
35    }
36}
37
38fn imu_bias_params_are_all_zero(params: &Params) -> bool {
39    [
40        ParamId::PARAM_ACC_X_BIAS,
41        ParamId::PARAM_ACC_Y_BIAS,
42        ParamId::PARAM_ACC_Z_BIAS,
43        ParamId::PARAM_GYRO_X_BIAS,
44        ParamId::PARAM_GYRO_Y_BIAS,
45        ParamId::PARAM_GYRO_Z_BIAS,
46    ]
47    .into_iter()
48    .all(|id| matches!(params.get_by_id(id), ParamValue::Float(value) if value == 0.0))
49}
50
51#[cfg(test)]
52mod tests {
53    use super::*;
54    use crate::{
55        packets::{ImuPacket, RosflightPacketHeader},
56        state_machine::Event,
57    };
58
59    const TEST_IMU_TIMEOUT_US: u64 = 100_000;
60
61    fn imu_packet(timestamp: u64) -> ImuPacket<f64> {
62        ImuPacket {
63            header: RosflightPacketHeader {
64                timestamp,
65                status: 0,
66            },
67            accel: [0.0, 0.0, -9.80665],
68            gyro: [0.0, 0.0, 0.0],
69            temperature: 25.0,
70            seq: 1,
71        }
72    }
73
74    #[test]
75    fn imu_calibration_health_sets_uncalibrated_error_when_all_bias_params_are_zero() {
76        let params = Params::new();
77        let mut state = StateManager::new();
78        state.update(Event::INITIALIZED, &params);
79        let mut last_imu_seen = 0;
80        let sensors = ProcessedSensors {
81            imu: Some(imu_packet(1)),
82            ..ProcessedSensors::<f64>::default()
83        };
84
85        update_sensor_health(SensorHealthCtx {
86            now_us: 1,
87            sensors: &sensors,
88            params: &params,
89            state: &mut state,
90            last_imu_seen: &mut last_imu_seen,
91            imu_timeout_us: TEST_IMU_TIMEOUT_US,
92        });
93
94        assert!(state.get_errors().contains(ErrorFlag::UNCALIBRATED_IMU));
95        assert_eq!(last_imu_seen, 1);
96    }
97
98    #[test]
99    fn imu_calibration_health_clears_uncalibrated_error_when_any_bias_param_is_nonzero() {
100        let mut params = Params::new();
101        params.set_by_id(ParamId::PARAM_ACC_X_BIAS, ParamValue::Float(0.01));
102        let mut state = StateManager::new();
103        state.update(Event::INITIALIZED, &params);
104        state.update(Event::ERROR_OCCURRED(ErrorFlag::UNCALIBRATED_IMU), &params);
105        let mut last_imu_seen = 0;
106        let sensors = ProcessedSensors {
107            imu: Some(imu_packet(1)),
108            ..ProcessedSensors::<f64>::default()
109        };
110
111        update_sensor_health(SensorHealthCtx {
112            now_us: 1,
113            sensors: &sensors,
114            params: &params,
115            state: &mut state,
116            last_imu_seen: &mut last_imu_seen,
117            imu_timeout_us: TEST_IMU_TIMEOUT_US,
118        });
119
120        assert!(!state.get_errors().contains(ErrorFlag::UNCALIBRATED_IMU));
121    }
122
123    #[test]
124    fn sensor_health_sets_imu_not_responding_after_timeout_without_imu_sample() {
125        let params = Params::new();
126        let mut state = StateManager::new();
127        state.update(Event::INITIALIZED, &params);
128        let mut last_imu_seen = 0;
129        let sensors = ProcessedSensors::<f64>::default();
130
131        update_sensor_health(SensorHealthCtx {
132            now_us: TEST_IMU_TIMEOUT_US + 1,
133            sensors: &sensors,
134            params: &params,
135            state: &mut state,
136            last_imu_seen: &mut last_imu_seen,
137            imu_timeout_us: TEST_IMU_TIMEOUT_US,
138        });
139
140        assert!(state.get_errors().contains(ErrorFlag::IMU_NOT_RESPONDING));
141        assert_eq!(last_imu_seen, 0);
142    }
143
144    #[test]
145    fn sensor_health_clears_imu_not_responding_when_imu_sample_returns() {
146        let params = Params::new();
147        let mut state = StateManager::new();
148        state.update(Event::INITIALIZED, &params);
149        state.update(
150            Event::ERROR_OCCURRED(ErrorFlag::IMU_NOT_RESPONDING),
151            &params,
152        );
153        let mut last_imu_seen = 0;
154        let sensors = ProcessedSensors {
155            imu: Some(imu_packet(5)),
156            ..ProcessedSensors::<f64>::default()
157        };
158
159        update_sensor_health(SensorHealthCtx {
160            now_us: 5,
161            sensors: &sensors,
162            params: &params,
163            state: &mut state,
164            last_imu_seen: &mut last_imu_seen,
165            imu_timeout_us: TEST_IMU_TIMEOUT_US,
166        });
167
168        assert!(!state.get_errors().contains(ErrorFlag::IMU_NOT_RESPONDING));
169        assert_eq!(last_imu_seen, 5);
170    }
171}
veloxity_core/sensors/health.rs

veloxity_core/sensors/
health.rs
