veloxity_core/
packets.rs

1pub static PARAM_PACKET_SIZE: usize = 2048;
2
3use crate::math::FlightFloat;
4
5pub const ADC_MAX_CHANNELS: usize = 21;
6pub const RC_PACKET_CHANNELS: usize = 24;
7
8#[derive(Debug, Clone, Copy, Default)]
9pub enum RangeType {
10    #[default]
11    Sonar,
12    Lidar,
13}
14
15#[derive(Debug, Clone, Copy, Default)]
16pub enum GNSSFixType {
17    #[default]
18    NoFix,
19    DeadReckoningOnly,
20    TwoD,
21    ThreeD,
22    GnssPlusDeadReckoning,
23    TimeFixOnly,
24}
25
26impl GNSSFixType {
27    pub fn from_u8(value: u8) -> Self {
28        match value {
29            0 => GNSSFixType::NoFix,
30            1 => GNSSFixType::DeadReckoningOnly,
31            2 => GNSSFixType::TwoD,
32            3 => GNSSFixType::ThreeD,
33            4 => GNSSFixType::GnssPlusDeadReckoning,
34            5 => GNSSFixType::TimeFixOnly,
35            _ => GNSSFixType::NoFix, // Handle unknown/invalid fix type
36        }
37    }
38}
39
40#[derive(Debug, Clone, Copy, Default)]
41pub struct RosflightPacketHeader {
42    // Microseconds; packets avoid board-specific timestamp types.
43    pub timestamp: u64,
44    pub status: u16,
45}
46
47#[derive(Debug, Clone, Copy, Default)]
48pub struct AdcPacket {
49    pub header: RosflightPacketHeader,
50    pub temperature: f32,
51    pub v_bku: f32,
52    pub v_ref: f32,
53    pub volts: [f32; ADC_MAX_CHANNELS],
54}
55
56#[derive(Debug, Clone, Copy, Default)]
57pub struct BatteryPacket {
58    pub header: RosflightPacketHeader,
59    pub voltage: f32,
60    pub current: f32,
61}
62
63#[derive(Debug, Clone, Copy, Default)]
64pub struct ImuPacket<R: FlightFloat> {
65    pub header: RosflightPacketHeader,
66    pub accel: [R; 3],
67    pub gyro: [R; 3],
68    pub temperature: f32,
69    pub seq: u32,
70}
71
72impl<R: FlightFloat> ImuPacket<R> {
73    pub fn cast<T: FlightFloat>(self) -> ImuPacket<T> {
74        ImuPacket {
75            header: self.header,
76            accel: [
77                <T as FlightFloat>::from_flight_float(self.accel[0]),
78                <T as FlightFloat>::from_flight_float(self.accel[1]),
79                <T as FlightFloat>::from_flight_float(self.accel[2]),
80            ],
81            gyro: [
82                <T as FlightFloat>::from_flight_float(self.gyro[0]),
83                <T as FlightFloat>::from_flight_float(self.gyro[1]),
84                <T as FlightFloat>::from_flight_float(self.gyro[2]),
85            ],
86            temperature: self.temperature,
87            seq: self.seq,
88        }
89    }
90}
91
92#[derive(Debug, Clone, Copy, Default)]
93pub struct BaroPacket {
94    pub header: RosflightPacketHeader,
95    pub pressure: f32,
96    pub temperature: f32,
97    pub altitude: f32,
98}
99
100#[derive(Debug, Clone, Copy, Default)]
101pub struct PitotPacket {
102    pub header: RosflightPacketHeader,
103    pub differential_pressure: f32,
104    pub temperature: f32,
105    pub indicated_airspeed: f32,
106}
107
108#[derive(Debug, Clone, Copy, Default)]
109pub struct MagPacket {
110    pub header: RosflightPacketHeader,
111    pub flux: [f32; 3],
112    pub temperature: f32,
113}
114
115#[derive(Debug, Clone, Copy, Default)]
116pub struct RcPacket {
117    pub header: RosflightPacketHeader,
118    pub n_chan: u32,
119    pub chan: [f32; RC_PACKET_CHANNELS],
120    pub lol: bool,
121}
122
123#[derive(Debug, Clone, Copy, Default)]
124pub struct RangePacket {
125    pub header: RosflightPacketHeader,
126    pub range: f32,
127    pub min_range: f32,
128    pub max_range: f32,
129    pub range_type: RangeType,
130}
131
132#[derive(Debug, Clone, Copy, Default)]
133pub struct GNSSPacket {
134    pub header: RosflightPacketHeader, // timestamp and device specific status
135    pub unix_seconds: i64,             // Unix time, in seconds
136    pub unix_nanos: i32,
137    pub lat: f64,    // degrees
138    pub lon: f64,    // degrees
139    pub height: f32, // m/s above ellipsoid
140    pub vel_n: f32,  // m/s north
141    pub vel_e: f32,  // m/s east
142    pub vel_d: f32,  // m/s down
143    pub h_acc: f32,  // m north/east
144    pub v_acc: f32,  // m down
145    pub s_acc: f32,  // m/s
146    pub month: u8,   // 0-11
147    pub year: u16,   // 0-65535 UTC
148    pub day: u8,     // 0-31 UTS day of month
149    pub hour: u8,    // 0-23 UTC
150    pub min: u8,     // 0-59 UTC
151    pub sec: u8,     // 0-59 UTC
152    pub nano: i32,   // adjustment +/1 to seconds
153    pub fix_type: GNSSFixType,
154    pub num_sats: u8, // 0-255
155    pub mag_dec: f32, // Magnetic Declination ??
156    pub time_correction: u64,
157}
158
159#[derive(Debug, Clone, Copy, Default)]
160pub struct PpsPacket {
161    pub header: RosflightPacketHeader,
162}
163
164#[derive(Debug, Clone, Copy, Default)]
165pub struct AttitudePacket {
166    pub header: RosflightPacketHeader,
167    pub q: [f32; 4],
168    pub rate: [f32; 3],
169}
170
171#[derive(Debug, Clone, Copy)]
172pub struct ParamPacket {
173    pub header: RosflightPacketHeader,
174    pub values: [u8; PARAM_PACKET_SIZE],
175}
176
177impl Default for ParamPacket {
178    fn default() -> Self {
179        Self {
180            header: RosflightPacketHeader::default(),
181            values: [0u8; PARAM_PACKET_SIZE],
182        }
183    }
184}
185
186#[cfg(test)]
187mod tests {
188    use super::*;
189
190    #[test]
191    fn imu_packet_cast_preserves_values_across_flight_float_types() {
192        let packet = ImuPacket::<f32> {
193            header: RosflightPacketHeader {
194                timestamp: 42,
195                status: 7,
196            },
197            accel: [1.0, -2.0, 3.5],
198            gyro: [0.1, -0.2, 0.3],
199            temperature: 24.0,
200            seq: 9,
201        };
202
203        let widened = packet.cast::<f64>();
204        assert_eq!(widened.header.timestamp, packet.header.timestamp);
205        assert_eq!(widened.header.status, packet.header.status);
206        assert_eq!(widened.accel, [1.0, -2.0, 3.5]);
207        assert_eq!(
208            widened.gyro,
209            [0.1_f32 as f64, -0.2_f32 as f64, 0.3_f32 as f64]
210        );
211        assert_eq!(widened.temperature, 24.0);
212        assert_eq!(widened.seq, 9);
213    }
214}
veloxity_core/packets.rs

veloxity_core/
packets.rs
