ublox/ubx_packets/packets/
esf_status.rs

1#![cfg(any(
2    feature = "ubx_proto23",
3    feature = "ubx_proto27",
4    feature = "ubx_proto31"
5))]
6#[allow(unused_imports, reason = "It is only unused in some feature sets")]
7use crate::FieldIter;
8use bitflags::bitflags;
9use core::fmt;
10#[cfg(feature = "serde")]
11use {super::SerializeUbxPacketFields, crate::serde::ser::SerializeMap};
12
13use crate::{error::ParserError, UbxPacketMeta};
14use ublox_derive::{ubx_extend, ubx_extend_bitflags, ubx_packet_recv};
15
16#[ubx_packet_recv]
17#[ubx(class = 0x10, id = 0x10, max_payload_len = 1240)]
18struct EsfStatus {
19    itow: u32,
20    /// Version is 2 for M8L spec
21    version: u8,
22
23    #[ubx(map_type = EsfInitStatus1, from = EsfInitStatus1)]
24    init_status1: u8,
25
26    #[ubx(map_type = EsfInitStatus2, from = EsfInitStatus2)]
27    init_status2: u8,
28
29    reserved1: [u8; 5],
30
31    #[ubx(map_type = EsfStatusFusionMode)]
32    fusion_mode: u8,
33
34    reserved2: [u8; 2],
35
36    num_sens: u8,
37
38    #[ubx(
39        map_type = EsfSensorStatusIter,
40        from = EsfSensorStatusIter::new,
41        is_valid = EsfSensorStatusIter::is_valid,
42        may_fail,
43    )]
44    data: [u8; 0],
45}
46
47#[ubx_extend]
48#[ubx(from, rest_reserved)]
49#[repr(u8)]
50#[derive(Debug, Copy, Clone, PartialEq, Eq)]
51pub enum EsfStatusFusionMode {
52    Initializing = 0,
53    Fusion = 1,
54    Suspended = 2,
55    Disabled = 3,
56}
57
58#[repr(transparent)]
59#[derive(Copy, Clone)]
60#[cfg_attr(feature = "serde", derive(serde::Serialize))]
61pub struct EsfInitStatus1(u8);
62
63impl EsfInitStatus1 {
64    const WHEEL_TICK_MASK: u8 = 0x03;
65    const MOUNTING_ANGLE_STATUS_MASK: u8 = 0x07;
66    const INS_STATUS_MASK: u8 = 0x03;
67
68    pub fn wheel_tick_init_status(self) -> EsfStatusWheelTickInit {
69        let bits = (self.0) & Self::WHEEL_TICK_MASK;
70        match bits {
71            0 => EsfStatusWheelTickInit::Off,
72            1 => EsfStatusWheelTickInit::Initializing,
73            2 => EsfStatusWheelTickInit::Initialized,
74            _ => EsfStatusWheelTickInit::Invalid,
75        }
76    }
77
78    pub fn wheel_tick_init_status_raw(self) -> u8 {
79        self.wheel_tick_init_status() as u8
80    }
81
82    pub fn mounting_angle_status(self) -> EsfStatusMountAngle {
83        let bits = (self.0 >> 2) & Self::MOUNTING_ANGLE_STATUS_MASK;
84        match bits {
85            0 => EsfStatusMountAngle::Off,
86            1 => EsfStatusMountAngle::Initializing,
87            2 | 3 => EsfStatusMountAngle::Initialized,
88            _ => EsfStatusMountAngle::Invalid,
89        }
90    }
91
92    pub fn mount_angle_status_raw(self) -> u8 {
93        self.mounting_angle_status() as u8
94    }
95
96    pub fn ins_initialization_status(self) -> EsfStatusInsInit {
97        let bits = (self.0 >> 5) & Self::INS_STATUS_MASK;
98        match bits {
99            0 => EsfStatusInsInit::Off,
100            1 => EsfStatusInsInit::Initializing,
101            2 => EsfStatusInsInit::Initialized,
102            _ => EsfStatusInsInit::Invalid,
103        }
104    }
105
106    pub fn ins_initialization_status_raw(self) -> u8 {
107        self.ins_initialization_status() as u8
108    }
109}
110
111impl fmt::Debug for EsfInitStatus1 {
112    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
113        f.debug_struct("initStatus1")
114            .field("wtInitStatus", &self.wheel_tick_init_status())
115            .field("mntAlgStatus", &self.mounting_angle_status())
116            .field("insInitStatus", &self.ins_initialization_status())
117            .finish()
118    }
119}
120
121#[repr(u8)]
122#[derive(Copy, Clone, Debug)]
123#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
124pub enum EsfStatusWheelTickInit {
125    Off = 0,
126    Initializing = 1,
127    Initialized = 2,
128    /// Only two bits are reserved for the init status
129    Invalid = 3,
130}
131
132#[repr(u8)]
133#[derive(Copy, Clone, Debug)]
134#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
135pub enum EsfStatusMountAngle {
136    Off = 0,
137    Initializing = 1,
138    Initialized = 2,
139    /// Only two bits are reserved for the init status
140    Invalid = 3,
141}
142
143#[repr(u8)]
144#[derive(Copy, Clone, Debug)]
145#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
146pub enum EsfStatusInsInit {
147    Off = 0,
148    Initializing = 1,
149    Initialized = 2,
150    /// Only two bits are reserved for the init status
151    Invalid = 3,
152}
153
154#[repr(transparent)]
155#[derive(Copy, Clone)]
156#[cfg_attr(feature = "serde", derive(serde::Serialize))]
157pub struct EsfInitStatus2(u8);
158
159impl EsfInitStatus2 {
160    pub fn imu_init_status_raw(self) -> u8 {
161        self.imu_init_status() as u8
162    }
163
164    pub fn imu_init_status(self) -> EsfStatusImuInit {
165        let bits = (self.0) & 0x02;
166        match bits {
167            0 => EsfStatusImuInit::Off,
168            1 => EsfStatusImuInit::Initializing,
169            2 => EsfStatusImuInit::Initialized,
170            _ => EsfStatusImuInit::Invalid,
171        }
172    }
173}
174
175impl fmt::Debug for EsfInitStatus2 {
176    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
177        f.debug_struct("initStatus2")
178            .field("imuInitStatus", &self.imu_init_status())
179            .finish()
180    }
181}
182
183#[repr(u8)]
184#[derive(Copy, Clone, Debug)]
185#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
186pub enum EsfStatusImuInit {
187    Off = 0,
188    Initializing = 1,
189    Initialized = 2,
190    /// Only two bits are reserved for the init status
191    Invalid = 3,
192}
193
194#[derive(Debug)]
195#[cfg_attr(feature = "serde", derive(serde::Serialize))]
196pub struct EsfSensorStatus {
197    sens_status1: SensorStatus1,
198    sens_status2: SensorStatus2,
199    freq: u16,
200    faults: EsfSensorFaults,
201}
202
203impl EsfSensorStatus {
204    pub fn freq(&self) -> u16 {
205        self.freq
206    }
207
208    pub fn faults(&self) -> EsfSensorFaults {
209        self.faults
210    }
211
212    pub fn faults_raw(&self) -> u8 {
213        self.faults().into_raw()
214    }
215
216    pub fn sensor_type(&self) -> EsfSensorType {
217        self.sens_status1.sensor_type
218    }
219
220    pub fn sensor_type_raw(&self) -> u8 {
221        self.sensor_type() as u8
222    }
223
224    pub fn sensor_used(&self) -> bool {
225        self.sens_status1.used
226    }
227
228    pub fn sensor_ready(&self) -> bool {
229        self.sens_status1.ready
230    }
231
232    pub fn calibration_status(&self) -> EsfSensorStatusCalibration {
233        self.sens_status2.calibration_status
234    }
235
236    pub fn calibration_status_raw(&self) -> u8 {
237        self.calibration_status() as u8
238    }
239
240    pub fn time_status(&self) -> EsfSensorStatusTime {
241        self.sens_status2.time_status
242    }
243
244    pub fn time_status_raw(&self) -> u8 {
245        self.time_status() as u8
246    }
247}
248
249#[derive(Clone, Debug)]
250pub struct EsfSensorStatusIter<'a>(core::slice::ChunksExact<'a, u8>);
251
252impl<'a> EsfSensorStatusIter<'a> {
253    const BLOCK_SIZE: usize = 4;
254    fn new(bytes: &'a [u8]) -> Self {
255        Self(bytes.chunks_exact(Self::BLOCK_SIZE))
256    }
257
258    fn is_valid(bytes: &[u8]) -> bool {
259        bytes.len() % Self::BLOCK_SIZE == 0
260    }
261}
262
263impl core::iter::Iterator for EsfSensorStatusIter<'_> {
264    type Item = EsfSensorStatus;
265
266    fn next(&mut self) -> Option<Self::Item> {
267        let chunk = self.0.next()?;
268        let data = u32::from_le_bytes(chunk[0..Self::BLOCK_SIZE].try_into().unwrap());
269        Some(EsfSensorStatus {
270            sens_status1: ((data & 0xFF) as u8).into(),
271            sens_status2: (((data >> 8) & 0xFF) as u8).into(),
272            freq: ((data >> 16) & 0xFF).try_into().unwrap(),
273            faults: (((data >> 24) & 0xFF) as u8).into(),
274        })
275    }
276}
277
278#[derive(Copy, Clone, Debug)]
279#[cfg_attr(feature = "serde", derive(serde::Serialize))]
280pub struct SensorStatus1 {
281    sensor_type: EsfSensorType,
282    used: bool,
283    ready: bool,
284}
285
286impl From<u8> for SensorStatus1 {
287    fn from(s: u8) -> Self {
288        let sensor_type: EsfSensorType = (s & 0x3F).into();
289        Self {
290            sensor_type,
291            used: (s >> 6) != 0,
292            ready: (s >> 7) != 0,
293        }
294    }
295}
296
297#[repr(u8)]
298#[derive(Copy, Clone, Debug)]
299#[cfg_attr(feature = "serde", derive(serde::Serialize))]
300pub enum EsfSensorType {
301    None = 0,
302    /// Angular acceleration in [deg/s]
303    GyroZ = 5,
304    /// Unitless (counter)
305    FrontLeftWheelTicks = 6,
306    /// Unitless (counter)
307    FrontRightWheelTicks = 7,
308    /// Unitless (counter)
309    RearLeftWheelTicks = 8,
310    /// Unitless (counter)
311    RearRightWheelTicks = 9,
312    /// Unitless (counter)
313    SpeedTick = 10,
314    /// Speed in [m/s]
315    Speed = 11,
316    /// Temperature Celsius \[deg\]
317    GyroTemp = 12,
318    /// Angular acceleration in [deg/s]
319    GyroY = 13,
320    /// Angular acceleration in [deg/s]
321    GyroX = 14,
322    /// Specific force in [m/s^2]
323    AccX = 16,
324    /// Specific force in [m/s^2]
325    AccY = 17,
326    /// Specific force in [m/s^2]
327    AccZ = 18,
328    Invalid = 19,
329}
330
331impl From<u8> for EsfSensorType {
332    fn from(orig: u8) -> Self {
333        match orig {
334            0 => EsfSensorType::None,
335            5 => EsfSensorType::GyroZ,
336            6 => EsfSensorType::FrontLeftWheelTicks,
337            7 => EsfSensorType::FrontRightWheelTicks,
338            8 => EsfSensorType::RearLeftWheelTicks,
339            9 => EsfSensorType::RearRightWheelTicks,
340            10 => EsfSensorType::SpeedTick,
341            11 => EsfSensorType::Speed,
342            12 => EsfSensorType::GyroTemp,
343            13 => EsfSensorType::GyroY,
344            14 => EsfSensorType::GyroX,
345            16 => EsfSensorType::AccX,
346            17 => EsfSensorType::AccY,
347            18 => EsfSensorType::AccZ,
348            _ => EsfSensorType::Invalid,
349        }
350    }
351}
352
353#[allow(dead_code)]
354#[derive(Copy, Clone, Debug)]
355#[cfg_attr(feature = "serde", derive(serde::Serialize))]
356pub struct SensorStatus2 {
357    pub(crate) calibration_status: EsfSensorStatusCalibration,
358    pub(crate) time_status: EsfSensorStatusTime,
359}
360
361impl From<u8> for SensorStatus2 {
362    fn from(s: u8) -> Self {
363        let calibration_status: EsfSensorStatusCalibration = (s & 0x03).into();
364        let time_status: EsfSensorStatusTime = ((s >> 2) & 0x03).into();
365        Self {
366            calibration_status,
367            time_status,
368        }
369    }
370}
371
372#[repr(u8)]
373#[derive(Copy, Clone, Debug)]
374#[cfg_attr(feature = "serde", derive(serde::Serialize))]
375pub enum EsfSensorStatusCalibration {
376    NotCalibrated = 0,
377    Calibrating = 1,
378    Calibrated = 2,
379    Invalid = 4,
380}
381
382impl From<u8> for EsfSensorStatusCalibration {
383    fn from(orig: u8) -> Self {
384        match orig {
385            0 => EsfSensorStatusCalibration::NotCalibrated,
386            1 => EsfSensorStatusCalibration::Calibrating,
387            2 | 3 => EsfSensorStatusCalibration::Calibrated,
388            _ => EsfSensorStatusCalibration::Invalid,
389        }
390    }
391}
392
393#[repr(u8)]
394#[derive(Copy, Clone, Debug)]
395#[cfg_attr(feature = "serde", derive(serde::Serialize))]
396pub enum EsfSensorStatusTime {
397    NoData = 0,
398    OnReceptionFirstByte = 1,
399    OnEventInput = 2,
400    TimeTagFromData = 3,
401    Invalid = 4,
402}
403
404impl From<u8> for EsfSensorStatusTime {
405    fn from(orig: u8) -> Self {
406        match orig {
407            0 => EsfSensorStatusTime::NoData,
408            1 => EsfSensorStatusTime::OnReceptionFirstByte,
409            2 => EsfSensorStatusTime::OnEventInput,
410            3 => EsfSensorStatusTime::TimeTagFromData,
411            _ => EsfSensorStatusTime::Invalid,
412        }
413    }
414}
415
416#[ubx_extend_bitflags]
417#[ubx(from, into_raw, rest_reserved)]
418bitflags! {
419#[derive(Debug, Default, Clone, Copy)]
420pub struct EsfSensorFaults: u8 {
421    const BAD_MEASUREMENT = 1;
422    const BAD_TIME_TAG = 2;
423    const MISSING_MEASUREMENT = 4;
424    const NOISY_MEASUREMENT = 8;
425}
426}
427
428impl From<u8> for EsfSensorFaults {
429    fn from(s: u8) -> Self {
430        Self::from_bits(s).unwrap_or(EsfSensorFaults::empty())
431    }
432}
ublox/ubx_packets/packets/esf_status.rs

ublox/ubx_packets/packets/
esf_status.rs
