pamoja_mavlink/link.rs
1//! The byte-stream link seam and an in-process autopilot to exercise it with no hardware.
2//!
3//! MAVLink runs over anything that moves bytes: a serial line to a flight controller, a
4//! UDP socket to a ground station, a radio. This module abstracts that as a single
5//! [`ByteLink`] trait, so the same logic drives all of them, and a real serial or UDP
6//! backend plugs into it later without touching the protocol above. A [`Connection`]
7//! pairs a link with a [`Parser`](crate::Parser) and, optionally, signing, so sending and receiving whole
8//! messages is one call each.
9//!
10//! To make the whole path testable with nothing plugged in, [`MemoryLink`] connects two
11//! connections through an in-memory pipe, and [`SitlAutopilot`] is a software-in-the-loop
12//! stand-in that heartbeats and answers commands the way a real autopilot would. This is
13//! the drone equivalent of the loopback transport and device simulators the rest of the
14//! SDK uses to run with zero hardware.
15//!
16//! This layer is available with the default `std` feature; the protocol core below it is
17//! `no_std`.
18
19use crate::dialect::{
20 self, CommandAck, CommandLong, Heartbeat, Message, MissionCount, MissionItemInt,
21 MissionRequest, MissionRequestInt, MissionRequestList,
22};
23use crate::error::{MavlinkError, Result};
24use crate::frame::{Frame, Header};
25use crate::protocol::mission::{MissionReceiver, MissionSender, ReceiverAction};
26use crate::signing::{Signer, Verifier};
27
28// Resolves a message id to its CRC_EXTRA through the common-dialect registry, so the
29// parser can validate frames off the link.
30fn crc_extra_for(msgid: u32) -> Option<u8> {
31 dialect::crc_extra(msgid)
32}
33
34/// A bidirectional byte stream: the seam a MAVLink [`Connection`] moves frames over.
35///
36/// Implemented here by [`MemoryLink`] for hardware-free testing; a serial port or UDP
37/// socket implements the same two methods to carry MAVLink over real links.
38pub trait ByteLink {
39 /// Reads available bytes into `buf`, returning how many were read.
40 ///
41 /// # Arguments
42 ///
43 /// * `buf` - the destination for the bytes read.
44 ///
45 /// # Returns
46 ///
47 /// The number of bytes read; `0` means the link has no more input.
48 ///
49 /// # Errors
50 ///
51 /// Returns a [`MavlinkError`] if the underlying link fails.
52 fn read(&mut self, buf: &mut [u8]) -> impl core::future::Future<Output = Result<usize>>;
53
54 /// Writes all of `data` to the link.
55 ///
56 /// # Arguments
57 ///
58 /// * `data` - the bytes to write.
59 ///
60 /// # Returns
61 ///
62 /// `Ok(())` once every byte has been handed to the link.
63 ///
64 /// # Errors
65 ///
66 /// Returns a [`MavlinkError`] if the underlying link fails.
67 fn write_all(&mut self, data: &[u8]) -> impl core::future::Future<Output = Result<()>>;
68}
69
70// The size of a read from the link into the connection's staging buffer.
71const READ_CHUNK: usize = 512;
72
73/// A MAVLink endpoint over a [`ByteLink`]: sends and receives whole messages, and signs
74/// and verifies them when configured to.
75///
76/// A connection owns its sending identity and sequence counter, a streaming [`Parser`](crate::Parser) for
77/// the bytes it reads, and optional signing. Attach a [`Signer`] to sign every outgoing
78/// frame and a [`Verifier`] to check every signed incoming one.
79pub struct Connection<L> {
80 link: L,
81 parser: crate::parser::Parser,
82 header: Header,
83 signer: Option<Signer>,
84 verifier: Option<Verifier>,
85 require_signed: bool,
86 staging: [u8; READ_CHUNK],
87 staged_len: usize,
88 staged_pos: usize,
89}
90
91impl<L: ByteLink> Connection<L> {
92 /// Creates a connection that sends as the given system and component.
93 ///
94 /// # Arguments
95 ///
96 /// * `link` - the byte stream to carry frames over.
97 /// * `system_id` - this endpoint's system id.
98 /// * `component_id` - this endpoint's component id.
99 ///
100 /// # Returns
101 ///
102 /// The connection, with signing off.
103 pub fn new(link: L, system_id: u8, component_id: u8) -> Self {
104 Connection {
105 link,
106 parser: crate::parser::Parser::new(),
107 header: Header::new(system_id, component_id, 0),
108 signer: None,
109 verifier: None,
110 require_signed: false,
111 staging: [0u8; READ_CHUNK],
112 staged_len: 0,
113 staged_pos: 0,
114 }
115 }
116
117 /// Signs every outgoing frame with `signer`.
118 ///
119 /// # Arguments
120 ///
121 /// * `signer` - the signer to stamp outgoing frames with.
122 ///
123 /// # Returns
124 ///
125 /// The connection, for chaining.
126 pub fn with_signer(mut self, signer: Signer) -> Self {
127 self.signer = Some(signer);
128 self
129 }
130
131 /// Verifies signed incoming frames with `verifier`, and rejects unsigned ones.
132 ///
133 /// # Arguments
134 ///
135 /// * `verifier` - the verifier to check incoming signed frames with.
136 ///
137 /// # Returns
138 ///
139 /// The connection, for chaining.
140 pub fn with_verifier(mut self, verifier: Verifier) -> Self {
141 self.verifier = Some(verifier);
142 self.require_signed = true;
143 self
144 }
145
146 /// Sends a typed message, signing it if a signer is attached.
147 ///
148 /// # Arguments
149 ///
150 /// * `message` - the message to send.
151 ///
152 /// # Returns
153 ///
154 /// `Ok(())` once the frame has been written to the link.
155 ///
156 /// # Errors
157 ///
158 /// Returns [`MavlinkError::PayloadTooLong`] if the message does not fit a frame, or a
159 /// link error from the underlying [`ByteLink`].
160 pub async fn send<M: Message>(&mut self, message: &M) -> Result<()> {
161 let mut payload = [0u8; crate::frame::MAX_PAYLOAD];
162 let len = message.encode(&mut payload);
163 let frame = match self.signer.as_mut() {
164 Some(signer) => signer.sign(self.header, M::ID, &payload[..len], M::CRC_EXTRA)?,
165 None => Frame::encode_v2(self.header, M::ID, &payload[..len], M::CRC_EXTRA)?,
166 };
167 self.link.write_all(frame.as_bytes()).await?;
168 self.header.sequence = self.header.sequence.wrapping_add(1);
169 Ok(())
170 }
171
172 /// Receives the next whole frame from the link, verifying its signature if required.
173 ///
174 /// # Returns
175 ///
176 /// The next valid frame.
177 ///
178 /// # Errors
179 ///
180 /// Returns [`MavlinkError::Closed`] if the link ends before a frame arrives,
181 /// [`MavlinkError::Unsigned`] if a signature is required but the frame is unsigned,
182 /// [`MavlinkError::BadSignature`] or [`MavlinkError::ReplayedTimestamp`] if a signed
183 /// frame does not verify, or a link error from the underlying [`ByteLink`].
184 pub async fn recv(&mut self) -> Result<Frame> {
185 loop {
186 while self.staged_pos < self.staged_len {
187 let byte = self.staging[self.staged_pos];
188 self.staged_pos += 1;
189 if let Some(frame) = self.parser.push_byte(byte, &crc_extra_for) {
190 if let Some(verifier) = self.verifier.as_mut() {
191 if frame.is_signed() {
192 verifier.verify(&frame)?;
193 } else if self.require_signed {
194 return Err(MavlinkError::Unsigned);
195 }
196 }
197 return Ok(frame);
198 }
199 }
200 let n = self.link.read(&mut self.staging).await?;
201 if n == 0 {
202 return Err(MavlinkError::Closed);
203 }
204 self.staged_len = n;
205 self.staged_pos = 0;
206 }
207 }
208
209 /// Returns a shared reference to the underlying link.
210 ///
211 /// # Returns
212 ///
213 /// The link.
214 pub fn link(&self) -> &L {
215 &self.link
216 }
217}
218
219/// An in-process byte link: one end of a bidirectional pipe between two connections.
220///
221/// [`pair`](MemoryLink::pair) makes two ends whose writes appear as the other's reads, so two
222/// [`Connection`]s (or a [`Vehicle`](crate::vehicle::Vehicle) and a [`SitlAutopilot`]) exchange
223/// frames with no socket and no hardware. A read awaits until bytes are available and reports
224/// end of input once the other end is dropped, so the two ends can run as concurrent tasks the
225/// way a real link's peers do.
226pub struct MemoryLink {
227 stream: tokio::io::DuplexStream,
228}
229
230impl MemoryLink {
231 /// Creates a connected pair of links.
232 ///
233 /// # Returns
234 ///
235 /// Two ends; bytes written to one are read from the other.
236 pub fn pair() -> (MemoryLink, MemoryLink) {
237 // A generous buffer so a burst of frames never blocks the writer in a test.
238 let (a, b) = tokio::io::duplex(64 * 1024);
239 (MemoryLink { stream: a }, MemoryLink { stream: b })
240 }
241}
242
243impl ByteLink for MemoryLink {
244 async fn read(&mut self, buf: &mut [u8]) -> Result<usize> {
245 use tokio::io::AsyncReadExt;
246 self.stream
247 .read(buf)
248 .await
249 .map_err(|_| MavlinkError::Closed)
250 }
251
252 async fn write_all(&mut self, data: &[u8]) -> Result<()> {
253 use tokio::io::AsyncWriteExt;
254 self.stream
255 .write_all(data)
256 .await
257 .map_err(|_| MavlinkError::Closed)
258 }
259}
260
261/// A hardware-free autopilot stand-in for software-in-the-loop testing.
262///
263/// It behaves like the parts of an autopilot a ground station first talks to: it emits a
264/// [`Heartbeat`] on demand, answers a [`CommandLong`] with a [`CommandAck`], and speaks both
265/// sides of the mission protocol, receiving an uploaded plan and serving it back on download.
266/// Wire it to one end of a [`MemoryLink::pair`] and drive a ground-station [`Connection`] or a
267/// [`Vehicle`](crate::vehicle::Vehicle) on the other to exercise the full connect, command,
268/// mission, and telemetry path in a test.
269pub struct SitlAutopilot {
270 connection: Connection<MemoryLink>,
271 mission: Vec<MissionItemInt>,
272 receiving: Option<(MissionReceiver, Vec<MissionItemInt>)>,
273}
274
275impl SitlAutopilot {
276 /// Creates a SITL autopilot on a link, sending as the given system and component.
277 ///
278 /// # Arguments
279 ///
280 /// * `link` - the autopilot's end of a linked pair.
281 /// * `system_id` - the vehicle's system id.
282 /// * `component_id` - the autopilot component id.
283 ///
284 /// # Returns
285 ///
286 /// The autopilot, with signing off.
287 pub fn new(link: MemoryLink, system_id: u8, component_id: u8) -> Self {
288 SitlAutopilot {
289 connection: Connection::new(link, system_id, component_id),
290 mission: Vec::new(),
291 receiving: None,
292 }
293 }
294
295 /// Preloads the plan the autopilot serves on a mission download.
296 ///
297 /// # Arguments
298 ///
299 /// * `items` - the mission items to store.
300 pub fn load_mission(&mut self, items: &[MissionItemInt]) {
301 self.mission = items.to_vec();
302 }
303
304 /// Signs the autopilot's outgoing frames and verifies incoming ones with the same key.
305 ///
306 /// # Arguments
307 ///
308 /// * `signer` - the signer for outgoing frames.
309 /// * `verifier` - the verifier for incoming signed frames.
310 ///
311 /// # Returns
312 ///
313 /// The autopilot, for chaining.
314 pub fn secured(mut self, signer: Signer, verifier: Verifier) -> Self {
315 self.connection = self.connection.with_signer(signer).with_verifier(verifier);
316 self
317 }
318
319 /// Emits a heartbeat announcing the vehicle as an active quadrotor.
320 ///
321 /// # Returns
322 ///
323 /// `Ok(())` once the heartbeat has been sent.
324 ///
325 /// # Errors
326 ///
327 /// Returns a link error if the heartbeat cannot be written.
328 pub async fn emit_heartbeat(&mut self) -> Result<()> {
329 let heartbeat = Heartbeat {
330 custom_mode: 0,
331 type_: dialect::mav_type::QUADROTOR,
332 autopilot: dialect::mav_autopilot::ARDUPILOTMEGA,
333 base_mode: dialect::mav_mode_flag::CUSTOM_MODE_ENABLED,
334 system_status: dialect::mav_state::ACTIVE,
335 mavlink_version: 3,
336 };
337 self.connection.send(&heartbeat).await
338 }
339
340 /// Reads one frame and answers it the way an autopilot would.
341 ///
342 /// A command is acknowledged as accepted; a mission upload is received and stored; a
343 /// mission download is served from the stored plan. Any other frame is read and left
344 /// unanswered. Call it in a loop to keep the autopilot responsive.
345 ///
346 /// # Returns
347 ///
348 /// The frame that was read.
349 ///
350 /// # Errors
351 ///
352 /// Returns the same errors as [`Connection::recv`] and [`Connection::send`].
353 pub async fn serve_once(&mut self) -> Result<Frame> {
354 let frame = self.connection.recv().await?;
355 let (sys, comp) = (frame.system_id(), frame.component_id());
356 match frame.message_id() {
357 CommandLong::ID => {
358 let command = CommandLong::decode(frame.payload())?;
359 let ack = CommandAck {
360 command: command.command,
361 result: dialect::mav_result::ACCEPTED,
362 progress: 0,
363 result_param2: 0,
364 target_system: sys,
365 target_component: comp,
366 };
367 self.connection.send(&ack).await?;
368 }
369 MissionCount::ID => {
370 let count = MissionCount::decode(frame.payload())?.count;
371 let mut receiver =
372 MissionReceiver::new(sys, comp, dialect::mav_mission_type::MISSION);
373 let buffer = Vec::with_capacity(count as usize);
374 self.step_receive(receiver.on_count(count), receiver, buffer)
375 .await?;
376 }
377 MissionItemInt::ID => {
378 if let Some((mut receiver, mut buffer)) = self.receiving.take() {
379 let item = MissionItemInt::decode(frame.payload())?;
380 let (accepted, action) = receiver.on_item(&item);
381 if let Some(item) = accepted {
382 buffer.push(item);
383 }
384 self.step_receive(action, receiver, buffer).await?;
385 }
386 }
387 MissionRequestList::ID => {
388 let count = MissionSender::new(
389 &self.mission,
390 sys,
391 comp,
392 dialect::mav_mission_type::MISSION,
393 )
394 .count();
395 self.connection.send(&count).await?;
396 }
397 MissionRequestInt::ID => {
398 let seq = MissionRequestInt::decode(frame.payload())?.seq;
399 self.serve_item(sys, comp, seq).await?;
400 }
401 MissionRequest::ID => {
402 let seq = MissionRequest::decode(frame.payload())?.seq;
403 self.serve_item(sys, comp, seq).await?;
404 }
405 _ => {}
406 }
407 Ok(frame)
408 }
409
410 // Applies one mission-receiver step: send the next request and keep receiving, or store the
411 // completed plan and send the acknowledgement.
412 async fn step_receive(
413 &mut self,
414 action: ReceiverAction,
415 receiver: MissionReceiver,
416 buffer: Vec<MissionItemInt>,
417 ) -> Result<()> {
418 match action {
419 ReceiverAction::Request(request) => {
420 self.connection.send(&request).await?;
421 self.receiving = Some((receiver, buffer));
422 }
423 ReceiverAction::Ack(ack) => {
424 self.mission = buffer;
425 self.connection.send(&ack).await?;
426 }
427 }
428 Ok(())
429 }
430
431 // Answers a request for one stored mission item.
432 async fn serve_item(&mut self, sys: u8, comp: u8, seq: u16) -> Result<()> {
433 let item = MissionSender::new(&self.mission, sys, comp, dialect::mav_mission_type::MISSION)
434 .item(seq);
435 if let Some(item) = item {
436 self.connection.send(&item).await?;
437 }
438 Ok(())
439 }
440}
441
442#[cfg(test)]
443mod tests {
444 use super::*;
445 use crate::signing::KEY_LEN;
446
447 const KEY: [u8; KEY_LEN] = [0x24; KEY_LEN];
448
449 fn arm_command() -> CommandLong {
450 CommandLong {
451 param1: 1.0,
452 param2: 0.0,
453 param3: 0.0,
454 param4: 0.0,
455 param5: 0.0,
456 param6: 0.0,
457 param7: 0.0,
458 command: dialect::mav_cmd::COMPONENT_ARM_DISARM,
459 target_system: 1,
460 target_component: 1,
461 confirmation: 0,
462 }
463 }
464
465 #[tokio::test]
466 async fn a_heartbeat_crosses_the_link() {
467 let (gcs_end, vehicle_end) = MemoryLink::pair();
468 let mut vehicle = SitlAutopilot::new(vehicle_end, 1, 1);
469 let mut gcs = Connection::new(gcs_end, 255, 190);
470
471 vehicle.emit_heartbeat().await.unwrap();
472 let frame = gcs.recv().await.unwrap();
473 assert_eq!(frame.message_id(), Heartbeat::ID);
474 let heartbeat = Heartbeat::decode(frame.payload()).unwrap();
475 assert_eq!(heartbeat.system_status, dialect::mav_state::ACTIVE);
476 }
477
478 #[tokio::test]
479 async fn a_command_is_answered_with_an_ack() {
480 let (gcs_end, vehicle_end) = MemoryLink::pair();
481 let mut vehicle = SitlAutopilot::new(vehicle_end, 1, 1);
482 let mut gcs = Connection::new(gcs_end, 255, 190);
483
484 gcs.send(&arm_command()).await.unwrap();
485 let served = vehicle.serve_once().await.unwrap();
486 assert_eq!(served.message_id(), CommandLong::ID);
487
488 let frame = gcs.recv().await.unwrap();
489 assert_eq!(frame.message_id(), CommandAck::ID);
490 let ack = CommandAck::decode(frame.payload()).unwrap();
491 assert_eq!(ack.command, dialect::mav_cmd::COMPONENT_ARM_DISARM);
492 assert_eq!(ack.result, dialect::mav_result::ACCEPTED);
493 }
494
495 #[tokio::test]
496 async fn a_signed_command_round_trips_over_the_link() {
497 let (gcs_end, vehicle_end) = MemoryLink::pair();
498 let mut vehicle = SitlAutopilot::new(vehicle_end, 1, 1)
499 .secured(Signer::new(KEY, 1, 10_000), Verifier::new(KEY));
500 let mut gcs = Connection::new(gcs_end, 255, 190)
501 .with_signer(Signer::new(KEY, 2, 20_000))
502 .with_verifier(Verifier::new(KEY));
503
504 gcs.send(&arm_command()).await.unwrap();
505 // The vehicle verifies the signed command before acting on it.
506 vehicle.serve_once().await.unwrap();
507 // The ground station verifies the signed acknowledgement.
508 let frame = gcs.recv().await.unwrap();
509 assert!(frame.is_signed());
510 assert_eq!(frame.message_id(), CommandAck::ID);
511 }
512
513 #[tokio::test]
514 async fn an_unsigned_frame_is_refused_when_signing_is_required() {
515 let (gcs_end, vehicle_end) = MemoryLink::pair();
516 // The vehicle requires signed frames; the ground station sends unsigned ones.
517 let mut vehicle = SitlAutopilot::new(vehicle_end, 1, 1)
518 .secured(Signer::new(KEY, 1, 10_000), Verifier::new(KEY));
519 let mut gcs = Connection::new(gcs_end, 255, 190);
520
521 gcs.send(&arm_command()).await.unwrap();
522 assert_eq!(vehicle.serve_once().await, Err(MavlinkError::Unsigned));
523 }
524}