1use crate::{
2 comm::messages::{
3 PARAM_READ_INGRESS_QUEUE_CAPACITY, PARAM_SET_BURST_QUEUE_CAPACITY,
4 PARAM_SET_EVENT_QUEUE_CAPACITY,
5 enums::{ParamIdentifier, RosflightCmd},
6 messages::{
7 ExternalAttitudeMsg, HeartbeatMsg, OffboardControlMsg, ParamValueMsg,
8 RosflightAuxCmdMsg, RosflightCmdAckMsg, RosflightHardErrorMsg, RosflightVersionMsg,
9 StatustextMsg,
10 },
11 },
12 params::{ParamId, ParamValue},
13};
14use heapless::Deque;
15
16#[derive(Debug, Clone, Copy, Eq, PartialEq)]
17pub enum EventQueueError {
18 Full,
19}
20
21pub struct EventQueue<T: Copy, const N: usize> {
22 items: Deque<T, N>,
23}
24
25impl<T: Copy, const N: usize> EventQueue<T, N> {
26 pub const fn new() -> Self {
27 Self {
28 items: Deque::new(),
29 }
30 }
31
32 pub fn push(&mut self, event: T) -> Result<(), EventQueueError> {
33 self.items
34 .push_back(event)
35 .map_err(|_| EventQueueError::Full)
36 }
37
38 pub fn push_or_log(&mut self, event: T, label: &str) -> bool {
39 if self.push(event).is_ok() {
40 true
41 } else {
42 crate::log_warn!("event queue full: {}", label);
43 false
44 }
45 }
46
47 pub fn pop(&mut self) -> Option<T> {
48 self.items.pop_front()
49 }
50
51 pub fn iter(&self) -> impl Iterator<Item = T> + '_ {
52 self.items.iter().copied()
53 }
54
55 pub fn clear(&mut self) {
56 self.items.clear();
57 }
58
59 pub fn len(&self) -> usize {
60 self.items.len()
61 }
62
63 pub fn is_empty(&self) -> bool {
64 self.items.is_empty()
65 }
66
67 pub fn is_full(&self) -> bool {
68 self.items.len() == N
69 }
70}
71
72impl<T: Copy, const N: usize> Default for EventQueue<T, N> {
73 fn default() -> Self {
74 Self::new()
75 }
76}
77
78#[derive(Debug, Clone, Copy, PartialEq)]
79pub struct ParamSetRequested {
80 pub value: ParamValue,
81 pub param_id_bytes: [u8; 16],
82}
83
84#[derive(Debug, Clone, Copy, PartialEq)]
85pub struct ParamReadRequested {
86 pub identifier: ParamIdentifier,
87}
88
89#[derive(Debug, Clone, Copy, PartialEq)]
90pub struct ParamChanged {
91 pub id: ParamId,
92 pub old: ParamValue,
93 pub new: ParamValue,
94 pub param_id_bytes: [u8; 16],
95}
96
97#[derive(Debug, Clone, Copy, PartialEq)]
98pub struct ParamListRequested;
99
100#[derive(Debug, Clone, Copy, PartialEq)]
101pub struct CalibrationRequested {
102 pub command: RosflightCmd,
103}
104
105#[derive(Debug, Clone, Copy)]
106pub struct OffboardControlRequested {
107 pub now_us: u64,
108 pub msg: OffboardControlMsg,
109}
110
111#[derive(Debug, Clone, Copy, PartialEq)]
112pub struct ParamDefaultsRequested {
113 pub command: RosflightCmd,
114}
115
116#[derive(Debug, Clone, Copy, PartialEq)]
117pub struct BoardCommandRequested {
118 pub command: RosflightCmd,
119}
120
121#[derive(Debug, Clone, Copy, PartialEq)]
122pub struct RcTrimCalibrationRequested {
123 pub command: RosflightCmd,
124}
125
126#[derive(Debug, Clone, Copy, PartialEq)]
127pub struct VersionRequested {
128 pub command: RosflightCmd,
129}
130
131#[derive(Debug, Clone, Copy, PartialEq)]
132pub struct ResetOriginRequested {
133 pub command: RosflightCmd,
134}
135
136#[derive(Debug, Clone, Copy, PartialEq)]
137pub struct ConfigInfoRequested {
138 pub command: RosflightCmd,
139}
140
141#[derive(Debug, Clone, Copy)]
142pub struct CompanionHeartbeatReceived {
143 pub msg: HeartbeatMsg,
144}
145
146#[derive(Debug, Clone, Copy)]
147pub struct AuxCommandReceived {
148 pub msg: RosflightAuxCmdMsg,
149}
150
151#[derive(Debug, Clone, Copy)]
152pub struct ExternalAttitudeReceived {
153 pub msg: ExternalAttitudeMsg,
154}
155
156#[derive(Debug, Clone, Copy)]
157pub enum CommResponse {
158 ParamValue(ParamValueMsg),
159 CmdAck(RosflightCmdAckMsg),
160 Version(RosflightVersionMsg),
161 Statustext(StatustextMsg),
162 HardError(RosflightHardErrorMsg),
163}
164
165pub const PARAM_SET_REQUEST_QUEUE_CAPACITY: usize = PARAM_SET_EVENT_QUEUE_CAPACITY;
166pub const PARAM_READ_REQUEST_QUEUE_CAPACITY: usize = PARAM_READ_INGRESS_QUEUE_CAPACITY;
167pub const PARAM_LIST_REQUEST_QUEUE_CAPACITY: usize = 2;
168pub const PARAM_CHANGED_QUEUE_CAPACITY: usize = 8;
169pub const COMM_RESPONSE_QUEUE_CAPACITY: usize = PARAM_SET_BURST_QUEUE_CAPACITY;
170pub const CALIBRATION_REQUEST_QUEUE_CAPACITY: usize = 4;
171pub const OFFBOARD_CONTROL_REQUEST_QUEUE_CAPACITY: usize = 4;
172pub const PARAM_DEFAULTS_REQUEST_QUEUE_CAPACITY: usize = 2;
173pub const BOARD_COMMAND_REQUEST_QUEUE_CAPACITY: usize = 4;
174pub const RC_TRIM_CALIBRATION_REQUEST_QUEUE_CAPACITY: usize = 2;
175pub const VERSION_REQUEST_QUEUE_CAPACITY: usize = 2;
176pub const RESET_ORIGIN_REQUEST_QUEUE_CAPACITY: usize = 2;
177pub const CONFIG_INFO_REQUEST_QUEUE_CAPACITY: usize = 2;
178pub const COMPANION_HEARTBEAT_QUEUE_CAPACITY: usize = 2;
179pub const AUX_COMMAND_QUEUE_CAPACITY: usize = 2;
180pub const EXTERNAL_ATTITUDE_QUEUE_CAPACITY: usize = 2;
181
182#[derive(Default)]
183pub struct ParamEventQueues {
184 pub set_requests: EventQueue<ParamSetRequested, PARAM_SET_REQUEST_QUEUE_CAPACITY>,
185 pub read_requests: EventQueue<ParamReadRequested, PARAM_READ_REQUEST_QUEUE_CAPACITY>,
186 pub list_requests: EventQueue<ParamListRequested, PARAM_LIST_REQUEST_QUEUE_CAPACITY>,
187 pub changes: EventQueue<ParamChanged, PARAM_CHANGED_QUEUE_CAPACITY>,
188 pub full_refresh: bool,
189}
190
191#[derive(Default)]
192pub struct CommEventQueues {
193 pub responses: EventQueue<CommResponse, COMM_RESPONSE_QUEUE_CAPACITY>,
194}
195
196#[derive(Default)]
197pub struct CompanionEventQueues {
198 pub heartbeats: EventQueue<CompanionHeartbeatReceived, COMPANION_HEARTBEAT_QUEUE_CAPACITY>,
199 pub aux_commands: EventQueue<AuxCommandReceived, AUX_COMMAND_QUEUE_CAPACITY>,
200 pub external_attitudes: EventQueue<ExternalAttitudeReceived, EXTERNAL_ATTITUDE_QUEUE_CAPACITY>,
201}
202
203#[derive(Default)]
204pub struct CommandEventQueues {
205 pub calibration_requests: EventQueue<CalibrationRequested, CALIBRATION_REQUEST_QUEUE_CAPACITY>,
206 pub offboard_control_requests:
207 EventQueue<OffboardControlRequested, OFFBOARD_CONTROL_REQUEST_QUEUE_CAPACITY>,
208 pub param_defaults_requests:
209 EventQueue<ParamDefaultsRequested, PARAM_DEFAULTS_REQUEST_QUEUE_CAPACITY>,
210 pub board_command_requests:
211 EventQueue<BoardCommandRequested, BOARD_COMMAND_REQUEST_QUEUE_CAPACITY>,
212 pub rc_trim_calibration_requests:
213 EventQueue<RcTrimCalibrationRequested, RC_TRIM_CALIBRATION_REQUEST_QUEUE_CAPACITY>,
214 pub version_requests: EventQueue<VersionRequested, VERSION_REQUEST_QUEUE_CAPACITY>,
215 pub reset_origin_requests:
216 EventQueue<ResetOriginRequested, RESET_ORIGIN_REQUEST_QUEUE_CAPACITY>,
217 pub config_info_requests: EventQueue<ConfigInfoRequested, CONFIG_INFO_REQUEST_QUEUE_CAPACITY>,
218}
219
220impl ParamEventQueues {
221 pub fn is_empty(&self) -> bool {
222 self.set_requests.is_empty()
223 && self.read_requests.is_empty()
224 && self.list_requests.is_empty()
225 && self.changes.is_empty()
226 && !self.full_refresh
227 }
228
229 pub fn clear_loop_events(&mut self) {
230 self.set_requests.clear();
231 self.read_requests.clear();
232 self.list_requests.clear();
233 self.changes.clear();
234 self.full_refresh = false;
235 }
236}
237
238impl CommEventQueues {
239 pub fn is_empty(&self) -> bool {
240 self.responses.is_empty()
241 }
242
243 pub fn clear_loop_events(&mut self) {
244 self.responses.clear();
245 }
246}
247
248impl CompanionEventQueues {
249 pub fn is_empty(&self) -> bool {
250 self.heartbeats.is_empty()
251 && self.aux_commands.is_empty()
252 && self.external_attitudes.is_empty()
253 }
254
255 pub fn clear_loop_events(&mut self) {
256 self.heartbeats.clear();
257 self.aux_commands.clear();
258 self.external_attitudes.clear();
259 }
260}
261
262impl CommandEventQueues {
263 pub fn is_empty(&self) -> bool {
264 self.calibration_requests.is_empty()
265 && self.offboard_control_requests.is_empty()
266 && self.param_defaults_requests.is_empty()
267 && self.board_command_requests.is_empty()
268 && self.rc_trim_calibration_requests.is_empty()
269 && self.version_requests.is_empty()
270 && self.reset_origin_requests.is_empty()
271 && self.config_info_requests.is_empty()
272 }
273}
274
275#[cfg(test)]
276mod tests {
277 use super::*;
278 use crate::log::Logger;
279
280 #[test]
281 fn event_queue_preserves_fifo_order_across_wraparound() {
282 let mut queue = EventQueue::<u8, 3>::new();
283
284 assert_eq!(queue.push(1), Ok(()));
285 assert_eq!(queue.push(2), Ok(()));
286 assert_eq!(queue.pop(), Some(1));
287 assert_eq!(queue.push(3), Ok(()));
288 assert_eq!(queue.push(4), Ok(()));
289 assert_eq!(queue.push(5), Err(EventQueueError::Full));
290
291 assert_eq!(queue.pop(), Some(2));
292 assert_eq!(queue.pop(), Some(3));
293 assert_eq!(queue.pop(), Some(4));
294 assert_eq!(queue.pop(), None);
295 }
296
297 #[test]
298 fn event_queue_iter_reads_without_draining() {
299 let mut queue = EventQueue::<u8, 3>::new();
300
301 let _ = queue.push(7);
302 let _ = queue.push(8);
303
304 {
305 let mut iter = queue.iter();
306 assert_eq!(iter.next(), Some(7));
307 assert_eq!(iter.next(), Some(8));
308 assert_eq!(iter.next(), None);
309 }
310
311 assert_eq!(queue.pop(), Some(7));
312 assert_eq!(queue.pop(), Some(8));
313 }
314
315 #[test]
316 fn push_or_log_drops_new_event_when_queue_is_full() {
317 let mut queue = EventQueue::<u8, 1>::new();
318
319 assert!(queue.push_or_log(1, "test event"));
320 assert!(!queue.push_or_log(2, "test event"));
321
322 assert_eq!(queue.pop(), Some(1));
323 assert_eq!(queue.pop(), None);
324
325 while Logger::pop().is_some() {}
326 }
327}