veloxity_core/
packets.rs

1// ROSflight 2.0's persisted ARM `params_t` occupies 7,004 bytes. The SD driver
2// appends its four-byte CRC after this payload.
3pub static PARAM_PACKET_SIZE: usize = crate::params::storage::ROSFLIGHT_C_PARAM_STORAGE_SIZE;
4
5use crate::math::FlightFloat;
6
7pub const ADC_MAX_CHANNELS: usize = 21;
8pub const RC_PACKET_CHANNELS: usize = 24;
9
10#[derive(Debug, Clone, Copy, Default)]
11pub enum RangeType {
12    #[default]
13    Sonar,
14    Lidar,
15}
16
17#[derive(Debug, Clone, Copy, Default)]
18pub enum GNSSFixType {
19    #[default]
20    NoFix,
21    DeadReckoningOnly,
22    TwoD,
23    ThreeD,
24    GnssPlusDeadReckoning,
25    TimeFixOnly,
26}
27
28impl GNSSFixType {
29    pub fn from_u8(value: u8) -> Self {
30        match value {
31            0 => GNSSFixType::NoFix,
32            1 => GNSSFixType::DeadReckoningOnly,
33            2 => GNSSFixType::TwoD,
34            3 => GNSSFixType::ThreeD,
35            4 => GNSSFixType::GnssPlusDeadReckoning,
36            5 => GNSSFixType::TimeFixOnly,
37            _ => GNSSFixType::NoFix, // Handle unknown/invalid fix type
38        }
39    }
40}
41
42#[derive(Debug, Clone, Copy, Default)]
43pub struct RosflightPacketHeader {
44    // Microseconds; packets avoid board-specific timestamp types.
45    pub timestamp: u64,
46    pub status: u16,
47}
48
49/// Match ROSflight C's GNSS validity timestamp: use the receiver's signed UTC
50/// fractional second relative to the latest PPS edge, or estimate measurement
51/// time as 22 ms before packet completion when PPS/time validity is unavailable.
52pub fn rosflight_c_gnss_timestamp(
53    pps_timestamp_us: u64,
54    completion_timestamp_us: u64,
55    utc_nanos: i32,
56    time_and_fix_valid: bool,
57) -> u64 {
58    if pps_timestamp_us != 0 && pps_timestamp_us < completion_timestamp_us && time_and_fix_valid {
59        if utc_nanos < 0 {
60            pps_timestamp_us.saturating_sub(u64::from(utc_nanos.unsigned_abs()) / 1_000)
61        } else {
62            pps_timestamp_us.saturating_add(utc_nanos as u64 / 1_000)
63        }
64    } else {
65        completion_timestamp_us.saturating_sub(22_000)
66    }
67}
68
69#[derive(Debug, Clone, Copy, Default)]
70pub struct AdcPacket {
71    pub header: RosflightPacketHeader,
72    pub temperature: f32,
73    pub v_bku: f32,
74    pub v_ref: f32,
75    pub volts: [f32; ADC_MAX_CHANNELS],
76}
77
78#[derive(Debug, Clone, Copy, Default)]
79pub struct BatteryPacket {
80    pub header: RosflightPacketHeader,
81    pub voltage: f32,
82    pub current: f32,
83}
84
85#[derive(Debug, Clone, Copy, Default)]
86pub struct ImuPacket<R: FlightFloat> {
87    pub header: RosflightPacketHeader,
88    pub accel: [R; 3],
89    pub gyro: [R; 3],
90    pub temperature: f32,
91    pub seq: u32,
92}
93
94impl<R: FlightFloat> ImuPacket<R> {
95    pub fn cast<T: FlightFloat>(self) -> ImuPacket<T> {
96        ImuPacket {
97            header: self.header,
98            accel: [
99                <T as FlightFloat>::from_flight_float(self.accel[0]),
100                <T as FlightFloat>::from_flight_float(self.accel[1]),
101                <T as FlightFloat>::from_flight_float(self.accel[2]),
102            ],
103            gyro: [
104                <T as FlightFloat>::from_flight_float(self.gyro[0]),
105                <T as FlightFloat>::from_flight_float(self.gyro[1]),
106                <T as FlightFloat>::from_flight_float(self.gyro[2]),
107            ],
108            temperature: self.temperature,
109            seq: self.seq,
110        }
111    }
112}
113
114#[derive(Debug, Clone, Copy, Default)]
115pub struct BaroPacket {
116    pub header: RosflightPacketHeader,
117    pub pressure: f32,
118    pub temperature: f32,
119    pub altitude: f32,
120}
121
122#[derive(Debug, Clone, Copy, Default)]
123pub struct PitotPacket {
124    pub header: RosflightPacketHeader,
125    pub differential_pressure: f32,
126    pub temperature: f32,
127    pub indicated_airspeed: f32,
128}
129
130#[derive(Debug, Clone, Copy, Default)]
131pub struct MagPacket {
132    pub header: RosflightPacketHeader,
133    pub flux: [f32; 3],
134    pub temperature: f32,
135}
136
137#[derive(Debug, Clone, Copy, Default)]
138pub struct RcPacket {
139    pub header: RosflightPacketHeader,
140    pub n_chan: u32,
141    pub chan: [f32; RC_PACKET_CHANNELS],
142    pub lol: bool,
143}
144
145#[derive(Debug, Clone, Copy, Default)]
146pub struct RangePacket {
147    pub header: RosflightPacketHeader,
148    pub range: f32,
149    pub min_range: f32,
150    pub max_range: f32,
151    pub range_type: RangeType,
152}
153
154#[derive(Debug, Clone, Copy, Default)]
155pub struct GNSSPacket {
156    pub header: RosflightPacketHeader, // timestamp and device specific status
157    pub unix_seconds: i64,             // Unix time, in seconds
158    pub unix_nanos: i32,
159    pub lat: f64,        // degrees
160    pub lon: f64,        // degrees
161    pub height_msl: f32, // m above mean sea level
162    pub vel_n: f32,      // m/s north
163    pub vel_e: f32,      // m/s east
164    pub vel_d: f32,      // m/s down
165    pub h_acc: f32,      // m north/east
166    pub v_acc: f32,      // m down
167    pub s_acc: f32,      // m/s
168    pub month: u8,       // 0-11
169    pub year: u16,       // 0-65535 UTC
170    pub day: u8,         // 0-31 UTS day of month
171    pub hour: u8,        // 0-23 UTC
172    pub min: u8,         // 0-59 UTC
173    pub sec: u8,         // 0-59 UTC
174    pub nano: i32,       // adjustment +/1 to seconds
175    pub fix_type: GNSSFixType,
176    pub num_sats: u8, // 0-255
177    pub mag_dec: f32, // Magnetic Declination ??
178    pub time_correction: u64,
179}
180
181#[derive(Debug, Clone, Copy, Default)]
182pub struct PpsPacket {
183    pub header: RosflightPacketHeader,
184}
185
186#[derive(Debug, Clone, Copy, Default)]
187pub struct AttitudePacket {
188    pub header: RosflightPacketHeader,
189    pub q: [f32; 4],
190    pub rate: [f32; 3],
191}
192
193#[derive(Debug, Clone, Copy)]
194pub struct ParamPacket {
195    pub header: RosflightPacketHeader,
196    pub values: [u8; PARAM_PACKET_SIZE],
197}
198
199impl Default for ParamPacket {
200    fn default() -> Self {
201        Self {
202            header: RosflightPacketHeader::default(),
203            values: [0u8; PARAM_PACKET_SIZE],
204        }
205    }
206}
207
208#[cfg(test)]
209mod tests {
210    use super::*;
211
212    #[test]
213    fn c_gnss_timestamp_adds_positive_fraction_to_pps() {
214        assert_eq!(
215            rosflight_c_gnss_timestamp(2_000_000, 2_915_000, 900_000_000, true),
216            2_900_000
217        );
218    }
219
220    #[test]
221    fn c_gnss_timestamp_subtracts_negative_fraction_from_pps() {
222        assert_eq!(
223            rosflight_c_gnss_timestamp(3_000_000, 3_015_000, -100_000_000, true),
224            2_900_000
225        );
226    }
227
228    #[test]
229    fn c_gnss_timestamp_falls_back_without_valid_pps_time() {
230        assert_eq!(
231            rosflight_c_gnss_timestamp(0, 3_015_000, 0, false),
232            2_993_000
233        );
234    }
235
236    #[test]
237    fn imu_packet_cast_preserves_values_across_flight_float_types() {
238        let packet = ImuPacket::<f32> {
239            header: RosflightPacketHeader {
240                timestamp: 42,
241                status: 7,
242            },
243            accel: [1.0, -2.0, 3.5],
244            gyro: [0.1, -0.2, 0.3],
245            temperature: 24.0,
246            seq: 9,
247        };
248
249        let widened = packet.cast::<f64>();
250        assert_eq!(widened.header.timestamp, packet.header.timestamp);
251        assert_eq!(widened.header.status, packet.header.status);
252        assert_eq!(widened.accel, [1.0, -2.0, 3.5]);
253        assert_eq!(
254            widened.gyro,
255            [0.1_f32 as f64, -0.2_f32 as f64, 0.3_f32 as f64]
256        );
257        assert_eq!(widened.temperature, 24.0);
258        assert_eq!(widened.seq, 9);
259    }
260}
veloxity_core/packets.rs

veloxity_core/
packets.rs
