pamoja_mavlink/protocol/
frames.rs

1//! The service protocols driven by frames rather than decoded messages.
2//!
3//! Each machine in this module speaks in typed messages, which is the right level for the
4//! logic. A caller holding a link has frames, though, and turning one into the right call
5//! means decoding the payload, matching the message id, calling the machine, and encoding
6//! whatever it answers. That glue is the same every time, so it lives here once: a machine
7//! takes an incoming [`Frame`], and gives back the frame to send in reply and what happened.
8//!
9//! Like the machines themselves this has no IO, no timers, and no allocation; a frame is a
10//! fixed buffer of bytes.
11
12use crate::dialect::{
13    encode_message, mav_mission_result, CommandAck, Message, MissionAck, MissionCount,
14    MissionItemInt, MissionRequest, MissionRequestInt, MissionRequestList,
15};
16use crate::error::Result;
17use crate::frame::{Frame, Header};
18
19use super::{AckOutcome, CommandProtocol, MissionReceiver, MissionSender, ReceiverAction};
20
21/// What one incoming frame produced for a [`MissionReceiver`].
22#[derive(Clone, Copy, Debug)]
23pub struct ReceiverStep {
24    /// The item the frame carried, if it was the one expected next.
25    pub accepted: Option<MissionItemInt>,
26    /// The next thing to send, as the machine sees it.
27    pub action: ReceiverAction,
28    /// That same thing, ready for the link.
29    pub reply: Frame,
30}
31
32impl MissionReceiver {
33    /// Builds the frame that starts a download.
34    ///
35    /// # Arguments
36    ///
37    /// * `header` - the addressing fields to stamp on the frame.
38    ///
39    /// # Returns
40    ///
41    /// The `MISSION_REQUEST_LIST` frame.
42    ///
43    /// # Errors
44    ///
45    /// Returns [`MavlinkError::PayloadTooLong`](crate::MavlinkError::PayloadTooLong) if the
46    /// message does not fit a frame, which a request list never fails.
47    pub fn request_list_frame(&self, header: Header) -> Result<Frame> {
48        encode_message(header, &self.request_list())
49    }
50
51    /// Handles an incoming frame, if it is one this transfer is waiting for.
52    ///
53    /// A `MISSION_COUNT` opens the transfer and a `MISSION_ITEM_INT` advances it; any other
54    /// message is not this machine's to handle and is reported as such rather than refused,
55    /// so a caller can route one link's traffic through several machines.
56    ///
57    /// # Arguments
58    ///
59    /// * `frame` - the frame off the link.
60    /// * `header` - the addressing fields to stamp on the reply.
61    ///
62    /// # Returns
63    ///
64    /// The step taken, or [`None`] if the frame carries a message this transfer does not
65    /// handle.
66    ///
67    /// # Errors
68    ///
69    /// Returns an error only if the reply does not fit a frame, which no mission message can
70    /// cause; a short or long payload decodes the way the message layer defines.
71    ///
72    /// # Examples
73    ///
74    /// ```
75    /// use pamoja_mavlink::dialect::{encode_message, MissionCount, MissionItemInt};
76    /// use pamoja_mavlink::protocol::{MissionReceiver, ReceiverAction};
77    /// use pamoja_mavlink::Header;
78    ///
79    /// let vehicle = Header::new(1, 1, 0);
80    /// let station = Header::new(255, 190, 0);
81    /// let mut download = MissionReceiver::new(1, 1, 0);
82    ///
83    /// // The vehicle announces one item, and the receiver asks for it.
84    /// let count = MissionCount { count: 1, target_system: 255, target_component: 190, mission_type: 0, opaque_id: 0 };
85    /// let step = download
86    ///     .on_frame(&encode_message(vehicle, &count)?, station)?
87    ///     .expect("a count is handled");
88    /// assert!(matches!(step.action, ReceiverAction::Request(request) if request.seq == 0));
89    /// assert_eq!(step.reply.message_id(), 51); // MISSION_REQUEST_INT
90    ///
91    /// // A frame for some other machine is passed over rather than refused.
92    /// let heartbeat = pamoja_mavlink::dialect::Heartbeat { custom_mode: 0, type_: 2, autopilot: 3, base_mode: 0, system_status: 4, mavlink_version: 3 };
93    /// assert!(download.on_frame(&encode_message(vehicle, &heartbeat)?, station)?.is_none());
94    /// # Ok::<(), pamoja_mavlink::MavlinkError>(())
95    /// ```
96    pub fn on_frame(&mut self, frame: &Frame, header: Header) -> Result<Option<ReceiverStep>> {
97        let id = frame.message_id();
98        if id == MissionCount::ID {
99            let count = MissionCount::decode(frame.payload())?;
100            let action = self.on_count(count.count);
101            Ok(Some(ReceiverStep {
102                accepted: None,
103                reply: reply_of(&action, header)?,
104                action,
105            }))
106        } else if id == MissionItemInt::ID {
107            let item = MissionItemInt::decode(frame.payload())?;
108            let (accepted, action) = self.on_item(&item);
109            Ok(Some(ReceiverStep {
110                accepted,
111                reply: reply_of(&action, header)?,
112                action,
113            }))
114        } else {
115            Ok(None)
116        }
117    }
118}
119
120fn reply_of(action: &ReceiverAction, header: Header) -> Result<Frame> {
121    match action {
122        ReceiverAction::Request(request) => encode_message(header, request),
123        ReceiverAction::Ack(ack) => encode_message(header, ack),
124    }
125}
126
127/// What one incoming frame produced for a [`MissionSender`].
128///
129/// A reply carries a whole frame buffer, which dwarfs the other variant; boxing it would
130/// need an allocator, and this layer has none.
131#[derive(Clone, Copy, Debug)]
132#[allow(clippy::large_enum_variant)]
133pub enum SenderStep {
134    /// Send this frame: the count that opens the transfer, a requested item, or an error.
135    Reply(Frame),
136    /// The receiver has acknowledged the transfer with this
137    /// [`MAV_MISSION_RESULT`](crate::dialect::mav_mission_result); nothing more to send.
138    Finished(u8),
139}
140
141impl MissionSender<'_> {
142    /// Builds the frame that opens an upload.
143    ///
144    /// # Arguments
145    ///
146    /// * `header` - the addressing fields to stamp on the frame.
147    ///
148    /// # Returns
149    ///
150    /// The `MISSION_COUNT` frame.
151    ///
152    /// # Errors
153    ///
154    /// Returns [`MavlinkError::PayloadTooLong`](crate::MavlinkError::PayloadTooLong) if the
155    /// message does not fit a frame, which a count never fails.
156    pub fn count_frame(&self, header: Header) -> Result<Frame> {
157        encode_message(header, &self.count())
158    }
159
160    /// Handles an incoming frame, if it is one this transfer answers.
161    ///
162    /// A `MISSION_REQUEST_LIST` is answered with the count, a `MISSION_REQUEST_INT` (or the
163    /// older `MISSION_REQUEST`) with the item asked for, and a request past the end of the
164    /// plan with a `MISSION_ACK` reporting an invalid sequence. A `MISSION_ACK` from the
165    /// receiver ends the transfer. Any other message is reported as not handled.
166    ///
167    /// # Arguments
168    ///
169    /// * `frame` - the frame off the link.
170    /// * `header` - the addressing fields to stamp on the reply.
171    ///
172    /// # Returns
173    ///
174    /// The step taken, or [`None`] if the frame carries a message this transfer does not
175    /// handle.
176    ///
177    /// # Errors
178    ///
179    /// Returns an error only if the reply does not fit a frame, which no mission message can
180    /// cause; a short or long payload decodes the way the message layer defines.
181    ///
182    /// # Examples
183    ///
184    /// ```
185    /// use pamoja_mavlink::dialect::{encode_message, Message, MissionItemInt, MissionRequestInt};
186    /// use pamoja_mavlink::protocol::{MissionSender, SenderStep};
187    /// use pamoja_mavlink::Header;
188    ///
189    /// let waypoint = MissionItemInt { command: 16, x: -338_567_800, y: 1_512_153_000, z: 50.0, ..MissionItemInt::zeroed() };
190    /// let plan = [waypoint];
191    /// let upload = MissionSender::new(&plan, 1, 1, 0);
192    ///
193    /// // The vehicle asks for item 0 and gets it back, stamped with its sequence number.
194    /// let request = MissionRequestInt { seq: 0, target_system: 255, target_component: 190, mission_type: 0 };
195    /// let step = upload
196    ///     .on_frame(&encode_message(Header::new(1, 1, 0), &request)?, Header::new(255, 190, 0))?
197    ///     .expect("a request is handled");
198    /// let SenderStep::Reply(reply) = step else { panic!("an item is sent") };
199    /// assert_eq!(MissionItemInt::decode(reply.payload())?.seq, 0);
200    /// # Ok::<(), pamoja_mavlink::MavlinkError>(())
201    /// ```
202    pub fn on_frame(&self, frame: &Frame, header: Header) -> Result<Option<SenderStep>> {
203        let id = frame.message_id();
204        if id == MissionRequestList::ID {
205            MissionRequestList::decode(frame.payload())?;
206            encode_message(header, &self.count()).map(|frame| Some(SenderStep::Reply(frame)))
207        } else if id == MissionRequestInt::ID || id == MissionRequest::ID {
208            let seq = if id == MissionRequestInt::ID {
209                MissionRequestInt::decode(frame.payload())?.seq
210            } else {
211                MissionRequest::decode(frame.payload())?.seq
212            };
213            let reply = match self.item(seq) {
214                Some(item) => encode_message(header, &item)?,
215                None => encode_message(header, &self.refuse(mav_mission_result::INVALID_SEQUENCE))?,
216            };
217            Ok(Some(SenderStep::Reply(reply)))
218        } else if id == MissionAck::ID {
219            let ack = MissionAck::decode(frame.payload())?;
220            Ok(Some(SenderStep::Finished(ack.type_)))
221        } else {
222            Ok(None)
223        }
224    }
225
226    fn refuse(&self, result: u8) -> MissionAck {
227        let count = self.count();
228        MissionAck {
229            target_system: count.target_system,
230            target_component: count.target_component,
231            type_: result,
232            mission_type: count.mission_type,
233            ..MissionAck::zeroed()
234        }
235    }
236}
237
238impl CommandProtocol {
239    /// Classifies an incoming frame against the command in flight, if it is an acknowledgment.
240    ///
241    /// # Arguments
242    ///
243    /// * `frame` - the frame off the link.
244    ///
245    /// # Returns
246    ///
247    /// The outcome, or [`None`] if the frame is not a `COMMAND_ACK`.
248    ///
249    /// # Errors
250    ///
251    /// Never fails in practice; the `Result` mirrors the message layer, which zero-extends a
252    /// short payload and ignores the tail of a long one rather than refusing either.
253    ///
254    /// # Examples
255    ///
256    /// ```
257    /// use pamoja_mavlink::dialect::{encode_message, mav_cmd, mav_result, CommandAck};
258    /// use pamoja_mavlink::protocol::{AckOutcome, CommandProtocol};
259    /// use pamoja_mavlink::Header;
260    ///
261    /// let arm = CommandProtocol::new(mav_cmd::COMPONENT_ARM_DISARM, 3);
262    /// let ack = CommandAck { command: mav_cmd::COMPONENT_ARM_DISARM, result: mav_result::ACCEPTED, ..CommandAck::zeroed() };
263    /// let outcome = arm.on_frame(&encode_message(Header::new(1, 1, 0), &ack)?)?;
264    /// assert_eq!(outcome, Some(AckOutcome::Final(mav_result::ACCEPTED)));
265    /// # Ok::<(), pamoja_mavlink::MavlinkError>(())
266    /// ```
267    pub fn on_frame(&self, frame: &Frame) -> Result<Option<AckOutcome>> {
268        if frame.message_id() == CommandAck::ID {
269            Ok(Some(self.on_ack(&CommandAck::decode(frame.payload())?)))
270        } else {
271            Ok(None)
272        }
273    }
274}
275
276#[cfg(test)]
277mod tests {
278    use super::*;
279    use crate::dialect::{mav_cmd, mav_result};
280
281    const VEHICLE: Header = Header::new(1, 1, 0);
282    const STATION: Header = Header::new(255, 190, 0);
283
284    fn plan() -> [MissionItemInt; 2] {
285        [
286            MissionItemInt {
287                command: mav_cmd::NAV_TAKEOFF,
288                z: 20.0,
289                ..MissionItemInt::zeroed()
290            },
291            MissionItemInt {
292                command: mav_cmd::NAV_WAYPOINT,
293                x: -338_567_800,
294                y: 1_512_153_000,
295                z: 50.0,
296                ..MissionItemInt::zeroed()
297            },
298        ]
299    }
300
301    #[test]
302    fn a_whole_upload_runs_frame_to_frame() -> Result<()> {
303        let items = plan();
304        let upload = MissionSender::new(&items, 1, 1, 0);
305        let mut download = MissionReceiver::new(255, 190, 0);
306
307        // The station opens with a request list, and the vehicle answers with the count.
308        let opened = download.request_list_frame(STATION)?;
309        let Some(SenderStep::Reply(mut from_vehicle)) = upload.on_frame(&opened, VEHICLE)? else {
310            panic!("a request list is answered");
311        };
312        assert_eq!(from_vehicle.message_id(), MissionCount::ID);
313
314        // Each side answers the other until the receiver acknowledges.
315        let mut accepted = 0;
316        loop {
317            let step = download
318                .on_frame(&from_vehicle, STATION)?
319                .expect("the vehicle only sends what the receiver handles");
320            if step.accepted.is_some() {
321                accepted += 1;
322            }
323            match upload.on_frame(&step.reply, VEHICLE)? {
324                Some(SenderStep::Reply(next)) => from_vehicle = next,
325                Some(SenderStep::Finished(result)) => {
326                    assert_eq!(result, mav_mission_result::ACCEPTED);
327                    break;
328                }
329                None => panic!("the receiver only sends what the sender handles"),
330            }
331        }
332        assert_eq!(accepted, 2);
333        assert!(download.is_complete());
334        Ok(())
335    }
336
337    #[test]
338    fn a_request_past_the_plan_is_refused_with_the_published_result() -> Result<()> {
339        let items = plan();
340        let upload = MissionSender::new(&items, 1, 1, 0);
341        let request = MissionRequestInt {
342            seq: 7,
343            target_system: 255,
344            target_component: 190,
345            mission_type: 0,
346        };
347        let Some(SenderStep::Reply(reply)) =
348            upload.on_frame(&encode_message(VEHICLE, &request)?, STATION)?
349        else {
350            panic!("a request is answered");
351        };
352        assert_eq!(reply.message_id(), MissionAck::ID);
353        assert_eq!(
354            MissionAck::decode(reply.payload())?.type_,
355            mav_mission_result::INVALID_SEQUENCE
356        );
357        Ok(())
358    }
359
360    #[test]
361    fn the_older_request_message_is_answered_too() -> Result<()> {
362        let items = plan();
363        let upload = MissionSender::new(&items, 1, 1, 0);
364        let request = MissionRequest {
365            seq: 1,
366            target_system: 255,
367            target_component: 190,
368            mission_type: 0,
369        };
370        let Some(SenderStep::Reply(reply)) =
371            upload.on_frame(&encode_message(VEHICLE, &request)?, STATION)?
372        else {
373            panic!("a request is answered");
374        };
375        assert_eq!(MissionItemInt::decode(reply.payload())?.seq, 1);
376        Ok(())
377    }
378
379    #[test]
380    fn a_frame_for_another_machine_is_passed_over() -> Result<()> {
381        let items = plan();
382        let upload = MissionSender::new(&items, 1, 1, 0);
383        let mut download = MissionReceiver::new(255, 190, 0);
384        let arm = CommandProtocol::new(mav_cmd::COMPONENT_ARM_DISARM, 3);
385
386        let ack = CommandAck {
387            command: mav_cmd::COMPONENT_ARM_DISARM,
388            result: mav_result::IN_PROGRESS,
389            progress: 40,
390            ..CommandAck::zeroed()
391        };
392        let frame = encode_message(VEHICLE, &ack)?;
393
394        assert!(upload.on_frame(&frame, STATION)?.is_none());
395        assert!(download.on_frame(&frame, STATION)?.is_none());
396        assert_eq!(arm.on_frame(&frame)?, Some(AckOutcome::InProgress(40)));
397        Ok(())
398    }
399}
pamoja_mavlink/protocol/frames.rs

pamoja_mavlink/protocol/
frames.rs
