veloxity_core/comm/
messages.rs

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

veloxity_core/comm/
messages.rs
