veloxity_core/comm/
messages.rs

1use bitflags::bitflags;
2use heapless::Deque;
3use messages::*;
4
5#[cfg(feature = "runtime-diagnostics")]
6use core::sync::atomic::{AtomicU32, Ordering};
7
8#[cfg(feature = "runtime-diagnostics")]
9#[unsafe(no_mangle)]
10pub static VELOXITY_DIAG_TIMESYNC_REQUEST_RECEIVED: AtomicU32 = AtomicU32::new(0);
11#[cfg(feature = "runtime-diagnostics")]
12#[unsafe(no_mangle)]
13pub static VELOXITY_DIAG_TIMESYNC_REQUEST_OVERWRITE: AtomicU32 = AtomicU32::new(0);
14
15// PARAM_SET has a two-stage path: decoded MAVLink ingress waits here, then the
16// comm system admits work into the ECS event queue while that queue has room.
17// Keep enough room for a companion computer to send a full parameter-table load
18// as one burst without dropping valid set requests.
19pub const PARAM_SET_BURST_QUEUE_CAPACITY: usize = 512;
20pub const PARAM_SET_INGRESS_QUEUE_CAPACITY: usize = PARAM_SET_BURST_QUEUE_CAPACITY;
21pub const PARAM_SET_EVENT_QUEUE_CAPACITY: usize = PARAM_SET_BURST_QUEUE_CAPACITY;
22pub const PARAM_READ_INGRESS_QUEUE_CAPACITY: usize = PARAM_SET_BURST_QUEUE_CAPACITY;
23
24#[derive(Default)]
25pub struct Messages {
26    pub heartbeat: Option<HeartbeatMsg>,
27    pub param_request_read: Deque<ParamRequestReadMsg, PARAM_READ_INGRESS_QUEUE_CAPACITY>,
28    pub param_request_list: Option<ParamRequestListMsg>,
29    pub param_set: Deque<ParamSetMsg, PARAM_SET_INGRESS_QUEUE_CAPACITY>,
30    pub timesync: Option<TimesyncMsg>,
31    pub offboard_control: Option<OffboardControlMsg>,
32    pub cmd: Option<RosflightCmdMsg>,
33    pub aux_cmd: Option<RosflightAuxCmdMsg>,
34    pub external_attitude: Option<ExternalAttitudeMsg>,
35    pub rc_raw: Option<RcChannelsMsg>,
36}
37
38impl Messages {
39    pub fn has_pending(&self) -> bool {
40        self.heartbeat.is_some()
41            || !self.param_request_read.is_empty()
42            || self.param_request_list.is_some()
43            || !self.param_set.is_empty()
44            || self.timesync.is_some()
45            || self.offboard_control.is_some()
46            || self.cmd.is_some()
47            || self.aux_cmd.is_some()
48            || self.external_attitude.is_some()
49            || self.rc_raw.is_some()
50    }
51}
52
53pub trait Store<T> {
54    fn store(&mut self, msg: T);
55    fn take(&mut self) -> Option<T>;
56}
57
58// implements store function for each message type. comm should only receive known messages
59macro_rules! impl_store {
60    ($ty:ty, $field:ident, $name:literal) => {
61        impl Store<$ty> for Messages {
62            fn store(&mut self, msg: $ty) {
63                let _ = self.$field.insert(msg);
64            }
65            fn take(&mut self) -> Option<$ty> {
66                self.$field.take()
67            }
68        }
69    };
70}
71
72// implemented for messages that will be received
73impl_store!(HeartbeatMsg, heartbeat, "heartbeat");
74impl Store<ParamRequestReadMsg> for Messages {
75    fn store(&mut self, msg: ParamRequestReadMsg) {
76        if self.param_request_read.push_back(msg).is_err() {
77            crate::log_warn!("message queue full: param_request_read");
78        }
79    }
80
81    fn take(&mut self) -> Option<ParamRequestReadMsg> {
82        self.param_request_read.pop_front()
83    }
84}
85impl_store!(
86    ParamRequestListMsg,
87    param_request_list,
88    "param_request_list"
89);
90impl Store<ParamSetMsg> for Messages {
91    fn store(&mut self, msg: ParamSetMsg) {
92        if self.param_set.push_back(msg).is_err() {
93            crate::log_warn!("message queue full: param_set");
94        }
95    }
96
97    fn take(&mut self) -> Option<ParamSetMsg> {
98        self.param_set.pop_front()
99    }
100}
101impl Store<TimesyncMsg> for Messages {
102    fn store(&mut self, msg: TimesyncMsg) {
103        #[cfg(feature = "runtime-diagnostics")]
104        if msg.tc1 == 0 {
105            VELOXITY_DIAG_TIMESYNC_REQUEST_RECEIVED.fetch_add(1, Ordering::Relaxed);
106            if self.timesync.is_some() {
107                VELOXITY_DIAG_TIMESYNC_REQUEST_OVERWRITE.fetch_add(1, Ordering::Relaxed);
108            }
109        }
110        let _ = self.timesync.insert(msg);
111    }
112
113    fn take(&mut self) -> Option<TimesyncMsg> {
114        self.timesync.take()
115    }
116}
117impl_store!(OffboardControlMsg, offboard_control, "offboard_control");
118impl_store!(RosflightCmdMsg, cmd, "cmd");
119impl_store!(RosflightAuxCmdMsg, aux_cmd, "aux_cmd");
120impl_store!(ExternalAttitudeMsg, external_attitude, "external_attitude");
121
122pub mod messages {
123    use super::enums::*;
124    use crate::{packets::GNSSFixType, params::ParamValue, state_machine::ErrorFlag};
125    // Heartbeat
126    // I don't think we need all these fields for the generic message but I'm leaving them for now
127    #[derive(Debug, Clone, Copy)]
128    pub struct HeartbeatMsg {
129        pub type_: u8,     // MAV_TYPE
130        pub autopilot: u8, // MAV_AUTOPILOT (not found in xml...)
131        pub base_mode: u8, // MAV_MODE_FLAG
132        pub custom_mode: u32,
133        pub system_status: u8,   // MAV_STATE
134        pub mavlink_version: u8, // V1
135    }
136
137    // Note I changed the MAVLink param messages to use ParamValue. These may need to change to fit the param system
138
139    #[derive(Debug, Clone, Copy)]
140    pub struct ParamRequestReadMsg {
141        pub target_system: u8,
142        pub target_component: u8,
143        pub param_identifier: ParamIdentifier,
144    }
145
146    #[derive(Debug, Clone, Copy)]
147    pub struct ParamRequestListMsg {
148        pub target_system: u8,
149        pub target_component: u8,
150    }
151
152    #[derive(Debug, Clone, Copy)]
153    pub struct ParamValueMsg {
154        pub param_id: [u8; 16],
155        pub param_value: ParamValue,
156        pub param_count: u16,
157        pub param_index: u16,
158    }
159
160    #[derive(Debug, Clone, Copy)]
161    pub struct ParamSetMsg {
162        pub target_system: u8,
163        pub target_component: u8,
164        pub param_id: [u8; 16],
165        pub param_value: ParamValue,
166    }
167
168    #[derive(Debug, Clone, Copy)]
169    pub struct AttitudeQuaternionMsg {
170        pub time_boot_ms: u32,
171        pub q1: f32,         // w
172        pub q2: f32,         // x
173        pub q3: f32,         // y
174        pub q4: f32,         // z
175        pub rollspeed: f32,  // (rad/s)
176        pub pitchspeed: f32, // (rad/s)
177        pub yawspeed: f32,   // (rad/s)
178    }
179
180    // This could be handled differently... choosing this for now. Used RC packet for ref
181    pub const RC_PACKET_CHANNELS: usize = 24;
182    #[derive(Debug, Clone, Copy)]
183    pub struct RcChannelsMsg {
184        pub time_boot_ms: u32,
185        pub chancount: u8,
186        pub channels: [u16; RC_PACKET_CHANNELS],
187        pub rssi: u8,
188    }
189
190    #[derive(Debug, Clone, Copy)]
191    pub struct TimesyncMsg {
192        pub tc1: i64,
193        pub ts1: i64,
194    }
195
196    #[derive(Debug, Clone, Copy)]
197    pub struct StatustextMsg {
198        pub severity: Severity,
199        pub text: [u8; 50],
200    }
201
202    // Custom ROSflight messages below here. Should be good to go out of the box
203
204    #[derive(Debug, Clone, Copy)]
205    pub struct OffboardControlMsg {
206        pub mode: OffboardControlMode,
207        pub ignore: OffboardControlIgnore,
208        pub qx: f32,
209        pub qy: f32,
210        pub qz: f32,
211        pub fx: f32,
212        pub fy: f32,
213        pub fz: f32,
214        pub passthrough: [f32; 4],
215    }
216
217    #[derive(Debug, Clone, Copy)]
218    pub struct SmallImuMsg {
219        pub time_boot_us: u64,
220        pub xacc: f32,
221        pub yacc: f32,
222        pub zacc: f32,
223        pub xgyro: f32,
224        pub ygyro: f32,
225        pub zgyro: f32,
226        pub temperature: f32,
227    }
228
229    #[derive(Debug, Clone, Copy)]
230    pub struct SmallMagMsg {
231        pub xmag: f32,
232        pub ymag: f32,
233        pub zmag: f32,
234    }
235
236    #[derive(Debug, Clone, Copy)]
237    pub struct SmallBaroMsg {
238        pub altitude: f32,    // (m)
239        pub pressure: f32,    // (Pa)
240        pub temperature: f32, // (K)
241    }
242
243    #[derive(Debug, Clone, Copy)]
244    pub struct DiffPressureMsg {
245        pub velocity: f32,      // (m/s)
246        pub diff_pressure: f32, // (Pa)
247        pub temperature: f32,   // (K)
248    }
249
250    #[derive(Debug, Clone, Copy)]
251    pub struct SmallRangeMsg {
252        pub type_: RosflightRangeType,
253        pub range: f32,     // (m)
254        pub max_range: f32, // (m)
255        pub min_range: f32, // (m)
256    }
257
258    #[derive(Debug, Clone, Copy)]
259    pub struct RosflightCmdMsg {
260        pub command: RosflightCmd,
261    }
262
263    #[derive(Debug, Clone, Copy)]
264    pub struct RosflightCmdAckMsg {
265        pub command: RosflightCmd,
266        pub success: RosflightCmdResponse,
267    }
268
269    #[derive(Debug, Clone, Copy)]
270    pub struct RosflightOutputRawMsg {
271        pub stamp: u64,
272        pub values: [f32; 14],
273    }
274
275    #[derive(Debug, Clone, Copy)]
276    pub struct RosflightStatusMsg {
277        pub armed: u8,
278        pub failsafe: u8,
279        pub rc_override: u16,
280        pub offboard: u8,
281        pub error_code: ErrorFlag,
282        pub control_mode: OffboardControlMode,
283        pub num_errors: i16,
284        pub loop_time_us: i16,
285    }
286
287    #[derive(Debug, Clone, Copy)]
288    pub struct RosflightVersionMsg {
289        pub version: [u8; 50],
290    }
291
292    #[derive(Debug, Clone, Copy)]
293    pub struct RosflightAuxCmdMsg {
294        pub type_array: [RosflightAuxCmdType; 14usize],
295        pub aux_cmd_array: [f32; 14],
296    }
297
298    #[derive(Debug, Clone, Copy)]
299    pub struct ExternalAttitudeMsg {
300        pub qw: f32,
301        pub qx: f32,
302        pub qy: f32,
303        pub qz: f32,
304    }
305
306    #[derive(Debug, Clone, Copy)]
307    pub struct RosflightHardErrorMsg {
308        pub error_code: u32,
309        pub pc: u32,
310        pub reset_count: u32,
311        pub do_rearm: u32,
312    }
313
314    #[derive(Debug, Clone, Copy)]
315    pub struct RosflightGnssMsg {
316        pub seconds: i64,
317        pub nanos: i32,
318        pub fix_type: GNSSFixType,
319        pub num_sat: u8,
320        pub lat: f64,                 // deg DDS format
321        pub lon: f64,                 // deg DDS format
322        pub height_msl: f32,          // (m) above mean sea level
323        pub vel_n: f32,               // (m/s)
324        pub vel_e: f32,               // (m/s)
325        pub vel_d: f32,               // (m/s)
326        pub h_acc: f32,               // (m)
327        pub v_acc: f32,               // (m)
328        pub s_acc: f32,               // (m)
329        pub rosflight_timestamp: u64, // us, estimated firmware timestamp for the time of validity of the gnss
330    }
331
332    #[derive(Debug, Clone, Copy)]
333    pub struct BatteryStatusMsg {
334        pub battery_voltage: f32,
335        pub battery_current: f32,
336    }
337
338    #[derive(Debug, Clone, Copy)]
339    pub enum DownlinkMessage {
340        Heartbeat(HeartbeatMsg),
341        ParamValue(ParamValueMsg),
342        Status(RosflightStatusMsg),
343        Timesync(TimesyncMsg),
344        Version(RosflightVersionMsg),
345        OutputRaw(RosflightOutputRawMsg),
346        Attitude(AttitudeQuaternionMsg),
347        Baro(SmallBaroMsg),
348        DiffPressure(DiffPressureMsg),
349        Imu(SmallImuMsg),
350        Mag(SmallMagMsg),
351        RcRaw(RcChannelsMsg),
352        Range(SmallRangeMsg),
353        Gnss(RosflightGnssMsg),
354        CmdAck(RosflightCmdAckMsg),
355        RcChannels(RcChannelsMsg),
356        BatteryStatus(BatteryStatusMsg),
357        Statustext(StatustextMsg),
358        HardError(RosflightHardErrorMsg),
359    }
360}
361
362// Enums
363
364pub mod enums {
365    use super::bitflags;
366
367    #[derive(Debug, Clone, Copy, PartialEq, Eq)]
368    pub enum RosflightCmd {
369        RcCalibration,
370        AccelCalibration,
371        GyroCalibration,
372        BaroCalibration,
373        AirspeedCalibration,
374        ReadParams,
375        WriteParams,
376        SetParamDefaults,
377        Reboot,
378        RebootToBootloader,
379        SendVersion,
380        ResetOrigin,
381        SendAllConfigInfos,
382    }
383
384    #[derive(Debug, Clone, Copy)]
385    pub enum RosflightCmdResponse {
386        RosflightCmdFailed,
387        RosflightCmdSuccess,
388    }
389
390    #[repr(u8)]
391    #[derive(Clone, Copy, Debug, PartialEq, Default)]
392    pub enum OffboardControlMode {
393        ModePassThrough = 0,
394        ModeRollratePitchrateYawrateThrottle = 1,
395        #[default]
396        ModeRollPitchYawrateThrottle = 2,
397    }
398
399    #[derive(Debug, Clone, Copy)]
400    pub enum RosflightAuxCmdType {
401        Disabled,
402        Servo,
403        Motor,
404    }
405
406    #[derive(Debug, Clone, Copy)]
407    pub enum Severity {
408        Emergency,
409        Alert,
410        Critical,
411        Error,
412        Warning,
413        Notice,
414        Info,
415        Debug,
416    }
417
418    bitflags! {
419        #[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
420        pub struct OffboardControlIgnore: u16 {
421            const IGNORE_FX = 1 << 0;
422            const IGNORE_FY = 1 << 1;
423            const IGNORE_FZ = 1 << 2;
424            const IGNORE_QX = 1 << 3;
425            const IGNORE_QY = 1 << 4;
426            const IGNORE_QZ = 1 << 5;
427            const IGNORE_PASS_0 = 1 << 6;
428            const IGNORE_PASS_1 = 1 << 7;
429            const IGNORE_PASS_2 = 1 << 8;
430            const IGNORE_PASS_3 = 1 << 9;
431        }
432    }
433
434    impl OffboardControlIgnore {
435        pub fn is_ignoring_qx(&self) -> bool {
436            self.intersects(Self::IGNORE_QX)
437        }
438
439        pub fn is_ignoring_qy(&self) -> bool {
440            self.intersects(Self::IGNORE_QY)
441        }
442
443        pub fn is_ignoring_qz(&self) -> bool {
444            self.intersects(Self::IGNORE_QZ)
445        }
446
447        pub fn is_ignoring_fx(&self) -> bool {
448            self.intersects(Self::IGNORE_FX)
449        }
450
451        pub fn is_ignoring_fy(&self) -> bool {
452            self.intersects(Self::IGNORE_FY)
453        }
454
455        pub fn is_ignoring_fz(&self) -> bool {
456            self.intersects(Self::IGNORE_FZ)
457        }
458    }
459
460    #[derive(Debug, Clone, Copy)]
461    pub enum GnssFixType {
462        GnssFixNoFix,
463        GnssFixDeadReckoningOnly,
464        GnssFix2dFix,
465        GnssFix3dFix,
466        GnssFixGnssPlusDeadReckoning,
467        GnssFixTimeFixOnly,
468    }
469
470    #[derive(Debug, Clone, Copy, Default)]
471    pub enum RosflightRangeType {
472        #[default]
473        RosflightRangeSonar,
474        RosflightRangeLidar,
475    }
476
477    #[derive(Debug, Clone, Copy)]
478    pub enum MavType {
479        Generic,
480        FixedWing,
481        Quadrotor,
482    }
483
484    #[derive(Debug, Clone, Copy, Default)]
485    pub enum LogLevel {
486        #[default]
487        Info,
488        Warn,
489        Error,
490    }
491
492    #[derive(Debug, Clone, Copy, PartialEq)]
493    pub enum ParamIdentifier {
494        ID([u8; 16]),
495        INDEX(i16),
496    }
497}
veloxity_core/comm/messages.rs

veloxity_core/comm/
messages.rs
