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

use crate::binrw_util;
use alloc::vec::Vec;
use binrw::binrw;
use core::fmt;
use num_enum::{FromPrimitive, IntoPrimitive};

/// Connection port / external sensor source.
///
/// Used by ExtSensorStatus, ExtSensorInfo, and VelSensorSetup to identify the
/// receiver port an external sensor is connected to.
#[derive(Debug, Clone, Copy, PartialEq, Eq, FromPrimitive, IntoPrimitive)]
#[repr(u8)]
pub enum ConnectionPort {
    Com1 = 0,
    Com2 = 1,
    Com3 = 2,
    Com4 = 3,
    Gpio = 4,
    Usb1 = 5,
    Usb2 = 6,
    Ip10 = 7,
    Ip11 = 8,
    Ip12 = 9,
    Ip13 = 10,
    Ip14 = 11,
    Ip15 = 12,
    Ip16 = 13,
    Ip17 = 14,
    Ips1 = 15,
    Ips2 = 16,
    Ips3 = 17,
    Ips4 = 18,
    Ips5 = 19,
    Ipr1 = 20,
    Ipr2 = 21,
    Ipr3 = 22,
    Ipr4 = 23,
    Ipr5 = 24,
    InternalSpi = 32,
    #[num_enum(catch_all)]
    Unknown(u8),
}

impl fmt::Display for ConnectionPort {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        match self {
            ConnectionPort::Com1 => write!(f, "COM1"),
            ConnectionPort::Com2 => write!(f, "COM2"),
            ConnectionPort::Com3 => write!(f, "COM3"),
            ConnectionPort::Com4 => write!(f, "COM4"),
            ConnectionPort::Gpio => write!(f, "GPIO"),
            ConnectionPort::Usb1 => write!(f, "USB1"),
            ConnectionPort::Usb2 => write!(f, "USB2"),
            ConnectionPort::Ip10 => write!(f, "IP10"),
            ConnectionPort::Ip11 => write!(f, "IP11"),
            ConnectionPort::Ip12 => write!(f, "IP12"),
            ConnectionPort::Ip13 => write!(f, "IP13"),
            ConnectionPort::Ip14 => write!(f, "IP14"),
            ConnectionPort::Ip15 => write!(f, "IP15"),
            ConnectionPort::Ip16 => write!(f, "IP16"),
            ConnectionPort::Ip17 => write!(f, "IP17"),
            ConnectionPort::Ips1 => write!(f, "IPS1"),
            ConnectionPort::Ips2 => write!(f, "IPS2"),
            ConnectionPort::Ips3 => write!(f, "IPS3"),
            ConnectionPort::Ips4 => write!(f, "IPS4"),
            ConnectionPort::Ips5 => write!(f, "IPS5"),
            ConnectionPort::Ipr1 => write!(f, "IPR1"),
            ConnectionPort::Ipr2 => write!(f, "IPR2"),
            ConnectionPort::Ipr3 => write!(f, "IPR3"),
            ConnectionPort::Ipr4 => write!(f, "IPR4"),
            ConnectionPort::Ipr5 => write!(f, "IPR5"),
            ConnectionPort::InternalSpi => write!(f, "Internal SPI"),
            ConnectionPort::Unknown(x) => write!(f, "Unknown: {x}"),
        }
    }
}

/// External sensor model.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
#[repr(u8)]
pub enum ExtSensorModel {
    #[default]
    Unknown = 0,
    SbgEllipse = 2,
    SbgEllipse2 = 5,
    Vn100 = 7,
    Adis1650x = 10,
    ZeroVelocity = 20,
    VelocityInput = 21,
}

impl From<u8> for ExtSensorModel {
    fn from(value: u8) -> Self {
        match value {
            2 => ExtSensorModel::SbgEllipse,
            5 => ExtSensorModel::SbgEllipse2,
            7 => ExtSensorModel::Vn100,
            10 => ExtSensorModel::Adis1650x,
            20 => ExtSensorModel::ZeroVelocity,
            21 => ExtSensorModel::VelocityInput,
            _ => ExtSensorModel::Unknown,
        }
    }
}

impl fmt::Display for ExtSensorModel {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        match self {
            ExtSensorModel::Unknown => write!(f, "Unknown"),
            ExtSensorModel::SbgEllipse => write!(f, "SBG Ellipse"),
            ExtSensorModel::SbgEllipse2 => write!(f, "SBG Ellipse 2"),
            ExtSensorModel::Vn100 => write!(f, "VN-100"),
            ExtSensorModel::Adis1650x => write!(f, "ADIS1650x"),
            ExtSensorModel::ZeroVelocity => write!(f, "Zero Velocity"),
            ExtSensorModel::VelocityInput => write!(f, "Velocity Input"),
        }
    }
}

// ExtSensorStatus Block 4223
#[binrw]
#[derive(Clone, Debug)]
pub struct ExtSensorStatus {
    #[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>,
    pub source: u8,
    pub sensor_model: u8,
    #[br(parse_with = binrw::helpers::until_eof)]
    pub data: Vec<u8>,
}

impl ExtSensorStatus {
    // ADIS1650x status flags (byte 0)
    pub const ADIS_ERROR: u8 = 0x01;
    pub const ADIS_SENSOR_FAILURE: u8 = 0x02;
    pub const ADIS_CLOCK_ERROR: u8 = 0x04;
    pub const ADIS_CRC_ERROR: u8 = 0x08;
    pub const ADIS_NO_DATA_WRONG_STATE: u8 = 0x10;
    pub const ADIS_TIMESTAMP_ERROR: u8 = 0x20;
    pub const ADIS_DATA_GAP: u8 = 0x40;
    pub const ADIS_UNEXPECTED_SAMPLES: u8 = 0x80;

    // ADIS1650x info type (byte 1)
    pub const ADIS_INFO_NONE: u8 = 0;
    pub const ADIS_INFO_DIAG_STAT: u8 = 1;
    pub const ADIS_INFO_CRC_FAIL_COUNT: u8 = 2;
    pub const ADIS_INFO_NO_DATA_SECONDS: u8 = 3;
    pub const ADIS_INFO_INTERNAL_STATE: u8 = 4;
    pub const ADIS_INFO_SAMPLES_LAST_PPS: u8 = 5;
    pub const ADIS_INFO_DRIFTING_SAMPLES: u8 = 6;
    pub const ADIS_INFO_CLOCK_ERROR_SAMPLES: u8 = 7;
}