libsbf 0.17.0

A no_std rust crate to parse Septentrio SBF Messages.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347

use crate::binrw_util;
use binrw::binrw;
use core::fmt;
use num_enum::{FromPrimitive, IntoPrimitive};
use super::att_euler::AttitudeMode;
use super::pvt_geodetic::{Datum, PvtMode};

/// Combined GNSS mode containing PVT mode (bits 0-3) and attitude mode (bits 4-7).
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct GnssMode {
    raw: u8,
    pub pvt_mode: PvtMode,
    pub attitude_mode: AttitudeMode,
}

impl GnssMode {
    /// Parse from raw gnss_mode byte.
    pub fn from_byte(value: u8) -> Self {
        Self {
            raw: value,
            pvt_mode: PvtMode::from(value),
            attitude_mode: AttitudeMode::from((value >> 4) as u16),
        }
    }

    /// Get the raw byte value.
    pub fn raw(&self) -> u8 {
        self.raw
    }
}

impl fmt::Display for GnssMode {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(f, "PVT: {}, Attitude: {}", self.pvt_mode, self.attitude_mode)
    }
}

/// INS coupling mode (bits 0-2 of info field).
#[derive(Debug, Clone, Copy, PartialEq, Eq, IntoPrimitive)]
#[repr(u8)]
pub enum INSCouplingMode {
    LooselyCoupled = 0,
    #[num_enum(catch_all)]
    Unknown(u8),
}

#[allow(clippy::derivable_impls)]
impl Default for INSCouplingMode {
    fn default() -> Self {
        INSCouplingMode::LooselyCoupled
    }
}

impl From<u16> for INSCouplingMode {
    fn from(value: u16) -> Self {
        match value & 0x07 {
            0 => INSCouplingMode::LooselyCoupled,
            x => INSCouplingMode::Unknown(x as u8),
        }
    }
}

/// Solution output location (bits 3-5 of info field).
#[derive(Debug, Clone, Copy, PartialEq, Eq, IntoPrimitive)]
#[repr(u8)]
pub enum INSSolutionLocation {
    MainGnssAntenna = 0,
    FirstPoi = 1,
    #[num_enum(catch_all)]
    Unknown(u8),
}

#[allow(clippy::derivable_impls)]
impl Default for INSSolutionLocation {
    fn default() -> Self {
        INSSolutionLocation::MainGnssAntenna
    }
}

impl From<u16> for INSSolutionLocation {
    fn from(value: u16) -> Self {
        match (value >> 3) & 0x07 {
            0 => INSSolutionLocation::MainGnssAntenna,
            1 => INSSolutionLocation::FirstPoi,
            x => INSSolutionLocation::Unknown(x as u8),
        }
    }
}

/// INS error codes.
#[derive(Debug, Clone, Copy, PartialEq, Eq, FromPrimitive, IntoPrimitive)]
#[repr(u8)]
pub enum INSError {
    None = 0,
    /// Position output prohibited due to export laws.
    PositionProhibited = 7,
    /// INS solution not requested by user.
    NotRequested = 20,
    /// Not enough valid external sensor measurements.
    NotEnoughSensorMeasurements = 21,
    /// Static alignment ongoing.
    StaticAlignmentOngoing = 23,
    /// Waiting for GNSS PVT.
    WaitingForGnssPvt = 24,
    /// In-motion alignment ongoing.
    InMotionAlignmentOngoing = 28,
    /// Waiting for GNSS heading.
    WaitingForGnssHeading = 29,
    /// Waiting for IMU to synchronize with PPS.
    WaitingForImuSync = 30,
    /// Standard deviation exceeds user limit (setINSStdDevMask).
    StdDevExceedsLimit = 31,
    /// Unsupported settings in INS.
    UnsupportedSettings = 32,
    /// Incorrect IMU orientation.
    IncorrectImuOrientation = 35,
    /// Unrecognized error code.
    #[num_enum(catch_all)]
    Unknown(u8),
}

#[allow(clippy::derivable_impls)]
impl Default for INSError {
    fn default() -> Self {
        INSError::None
    }
}

// INS Nav Geod Block 4226
#[binrw]
#[derive(Clone, Debug)]
pub struct INSNavGeod {
    #[br(map = binrw_util::map_u4)]
    #[bw(map = binrw_util::unmap_u4)]
    pub tow: Option<u32>,
    #[br(map = binrw_util::map_u2)]
    #[bw(map = binrw_util::unmap_u2)]
    pub wnc: Option<u16>,
    /// Bits 0-3: PVT mode, Bits 4-7: Attitude mode
    gnss_mode_raw: u8,
    pub error: u8,
    /// Bits 0-2: coupling mode, Bits 3-5: solution location, Bits 6-8: flags
    pub info: u16,
    #[br(map = binrw_util::map_u2)]
    #[bw(map = binrw_util::unmap_u2)]
    pub gnss_age: Option<u16>,
    #[br(map = binrw_util::map_f8)]
    #[bw(map = binrw_util::unmap_f8)]
    pub latitude: Option<f64>,
    #[br(map = binrw_util::map_f8)]
    #[bw(map = binrw_util::unmap_f8)]
    pub longitude: Option<f64>,
    #[br(map = binrw_util::map_f8)]
    #[bw(map = binrw_util::unmap_f8)]
    pub height: Option<f64>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub undulation: Option<f32>,
    #[br(map = binrw_util::map_u2)]
    #[bw(map = binrw_util::unmap_u2)]
    pub accuracy: Option<u16>,
    #[br(map = binrw_util::map_u2)]
    #[bw(map = binrw_util::unmap_u2)]
    pub latency: Option<u16>,
    #[br(map = binrw_util::map_datum)]
    #[bw(map = binrw_util::unmap_datum)]
    pub datum: Option<Datum>,
    _reserved: u8,
    // TODO: unpack into an SBList type so we know what INSNav Sub Blocks we can parse
    pub sb_list: u16,

    #[br(if(sb_list & 1 == 1))]
    pub pos_std_dev: Option<INSNavGeodPosStdDev>,
    #[br(if((sb_list >> 1) & 1 == 1))]
    pub att: Option<INSNavGeodAtt>,
    #[br(if((sb_list >> 2) & 1 == 1))]
    pub att_std_dev: Option<INSNavGeodAttStdDev>,
    #[br(if((sb_list >> 3) & 1 == 1))]
    pub vel: Option<INSNavGeodVel>,
    #[br(if((sb_list >> 4) & 1 == 1))]
    pub vel_std_dev: Option<INSNavGeodVelStdDev>,
    #[br(if((sb_list >> 5) & 1 == 1))]
    pub pos_cov: Option<INSNavGeodPosCov>,
    #[br(if((sb_list >> 6) & 1 == 1))]
    pub att_cov: Option<INSNavGeodAttCov>,
    #[br(if((sb_list >> 7) & 1 == 1))]
    pub vel_cov: Option<INSNavGeodVelCov>,
}

#[binrw]
#[derive(Clone, Debug)]
pub struct INSNavGeodPosStdDev {
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub longitude_std_dev: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub latitude_std_dev: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub height_std_dev: Option<f32>,
}

#[binrw]
#[derive(Clone, Debug)]
pub struct INSNavGeodAtt {
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub heading: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub pitch: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub roll: Option<f32>,
}

#[binrw]
#[derive(Clone, Debug)]
pub struct INSNavGeodAttStdDev {
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub heading_std_dev: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub pitch_std_dev: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub roll_std_dev: Option<f32>,
}

#[binrw]
#[derive(Clone, Debug)]
pub struct INSNavGeodVel {
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub ve: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub vn: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub vu: Option<f32>,
}

#[binrw]
#[derive(Clone, Debug)]
pub struct INSNavGeodVelStdDev {
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub ve_std_dev: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub vn_std_dev: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub vu_std_dev: Option<f32>,
}

#[binrw]
#[derive(Clone, Debug)]
pub struct INSNavGeodPosCov {
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub latitude_longitude_cov: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub latitude_height_cov: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub longitude_height_cov: Option<f32>,
}

#[binrw]
#[derive(Clone, Debug)]
pub struct INSNavGeodVelCov {
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub ve_vn_cov: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub ve_vu_cov: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub vn_vu_cov: Option<f32>,
}

#[binrw]
#[derive(Clone, Debug)]
pub struct INSNavGeodAttCov {
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub heading_pitch_cov: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub heading_roll_cov: Option<f32>,
    #[br(map = binrw_util::map_f4)]
    #[bw(map = binrw_util::unmap_f4)]
    pub pitch_roll_cov: Option<f32>,
}

impl INSNavGeod {
    /// Combined GNSS mode (PVT mode + attitude mode).
    pub fn gnss_mode(&self) -> GnssMode {
        GnssMode::from_byte(self.gnss_mode_raw)
    }

    /// Last GNSS PVT mode used by INS filter (bits 0-3 of gnss_mode).
    pub fn pvt_mode(&self) -> PvtMode {
        PvtMode::from(self.gnss_mode_raw & 0x0F)
    }

    /// Last GNSS Attitude mode used by INS filter (bits 4-7 of gnss_mode).
    pub fn attitude_mode(&self) -> AttitudeMode {
        AttitudeMode::from((self.gnss_mode_raw >> 4) as u16)
    }

    /// INS error code.
    pub fn ins_error(&self) -> INSError {
        INSError::from(self.error)
    }

    /// INS coupling mode (bits 0-2 of info).
    pub fn coupling_mode(&self) -> INSCouplingMode {
        INSCouplingMode::from(self.info)
    }

    /// Solution output location (bits 3-5 of info).
    pub fn solution_location(&self) -> INSSolutionLocation {
        INSSolutionLocation::from(self.info)
    }

    /// Bit 6: 180-degree heading ambiguity fixed.
    pub fn heading_ambiguity_fixed(&self) -> bool {
        self.info & (1 << 6) != 0
    }

    /// Bit 7: Zero-velocity constraints used.
    pub fn zero_velocity_constraints(&self) -> bool {
        self.info & (1 << 7) != 0
    }

    /// Bit 8: IMU orientation estimation has converged.
    pub fn imu_orientation_converged(&self) -> bool {
        self.info & (1 << 8) != 0
    }
}