public/kaptan
1
Fork 0
Code Issues 24 Pull Requests Actions Packages Projects Releases Wiki Activity

mavlink: harden production reception and transmission

Browse Source
This commit is contained in:
Onur Özkan committed 2026-08-09 13:57:23 +03:00
1 parent 01463fa878
commit 91110255ec
5 files changed
+269 -72
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

libs/kaptan-mavlink/Cargo.toml
+5 -2
View File
@@ -10,12 +10,15 @@ version.workspace = true
workspace = true
[features]
default = ["dialect-kaptan"]
# Common MAVLink telemetry/arm acknowledgement is the default surface.
default = ["dialect-kaptan", "sensor-telemetry"]
# Sensor-domain adapters are physical telemetry only.
sensor-telemetry = ["dep:kaptan-sensor"]
dialect-kaptan = []
[dependencies]
kaptan-dma = { path = "../kaptan-dma" }
kaptan-sensor = { path = "../kaptan-sensor" }
kaptan-sensor = { path = "../kaptan-sensor", optional = true }
kaptan-tx = { path = "../kaptan-tx" }
mavlink = { version = "0.18", default-features = false, features = ["embedded", "dialect-common"] }
mavlink-core = { version = "0.18", default-features = false, features = ["embedded"] }
libs/kaptan-mavlink/definitions/kaptan.xml
+1 -1
View File
@@ -6,4 +6,4 @@
<!-- Special enums and messages for Kaptan. -->
<enums/>
<messages/>
</mavlink>
</mavlink>
libs/kaptan-mavlink/src/encoder.rs
+20 -4
View File
@@ -1,13 +1,17 @@
//! MAVLink frame encoding API.
#[cfg(feature = "sensor-telemetry")]
use kaptan_sensor::imu::ImuSnapshot;
#[cfg(feature = "sensor-telemetry")]
use kaptan_sensor::optical_flow::OpticalFlowSnapshot;
#[cfg(feature = "sensor-telemetry")]
use kaptan_sensor::range::RangeSnapshot;
use mavlink::MessageData;
use crate::{MavlinkFrame, message};
/// Board-selected MAVLink metadata for a range sensor.
#[cfg(feature = "sensor-telemetry")]
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct DistanceSensorConfig {
/// MAVLink sensor instance identifier.
@@ -52,12 +56,12 @@ impl MavlinkEncoder {
raw
}
/// Encodes a Kaptan heartbeat with its current arming state.
/// Encodes a board-I/O heartbeat with its current arming state.
pub fn heartbeat(&mut self, system_status: message::MavState, armed: bool) -> MavlinkFrame {
let heartbeat = message::HEARTBEAT_DATA {
custom_mode: 0,
mavtype: message::MavType::MAV_TYPE_QUADROTOR,
autopilot: message::MavAutopilot::MAV_AUTOPILOT_GENERIC,
mavtype: message::MavType::MAV_TYPE_ONBOARD_CONTROLLER,
autopilot: message::MavAutopilot::MAV_AUTOPILOT_INVALID,
base_mode: if armed {
message::MavModeFlag::MAV_MODE_FLAG_SAFETY_ARMED
} else {
@@ -80,6 +84,7 @@ impl MavlinkEncoder {
}
/// Encodes a standard `RAW_IMU` frame.
#[cfg(feature = "sensor-telemetry")]
pub fn raw_imu_snapshot(&mut self, snapshot: &ImuSnapshot) -> MavlinkFrame {
let raw_imu = message::RAW_IMU_DATA {
time_usec: snapshot.timestamp.as_microseconds(),
@@ -101,6 +106,7 @@ impl MavlinkEncoder {
///
/// MAVLink represents distance in centimetres; the sensor domain keeps
/// millimetres so this conversion happens only at the protocol boundary.
#[cfg(feature = "sensor-telemetry")]
pub fn distance_sensor_snapshot(
&mut self,
snapshot: &RangeSnapshot,
@@ -126,6 +132,7 @@ impl MavlinkEncoder {
/// Encodes a standard `OPTICAL_FLOW` frame from a ground-referenced flow
/// observation.
#[cfg(feature = "sensor-telemetry")]
pub fn optical_flow_snapshot(
&mut self,
snapshot: &OpticalFlowSnapshot,
@@ -150,11 +157,17 @@ impl MavlinkEncoder {
#[cfg(test)]
mod tests {
use super::{DistanceSensorConfig, MavlinkEncoder};
#[cfg(feature = "sensor-telemetry")]
use super::DistanceSensorConfig;
use super::MavlinkEncoder;
use crate::message::MavState;
#[cfg(feature = "sensor-telemetry")]
use kaptan_sensor::imu::ImuSnapshot;
#[cfg(feature = "sensor-telemetry")]
use kaptan_sensor::optical_flow::OpticalFlowSnapshot;
#[cfg(feature = "sensor-telemetry")]
use kaptan_sensor::range::RangeSnapshot;
#[cfg(feature = "sensor-telemetry")]
use kaptan_time::Timestamp;
#[test]
@@ -180,6 +193,7 @@ mod tests {
assert_eq!(frame.raw_bytes()[17], MavState::MAV_STATE_CRITICAL as u8);
}
#[cfg(feature = "sensor-telemetry")]
#[test]
fn snapshot_adapter_uses_flight_data_timestamp_and_units() {
let mut encoder = MavlinkEncoder::new(1, 1);
@@ -203,6 +217,7 @@ mod tests {
assert_ne!(frame.raw_bytes()[16] & 0x80, 0);
}
#[cfg(feature = "sensor-telemetry")]
#[test]
fn range_snapshot_uses_distance_sensor_centimetres() {
let mut encoder = MavlinkEncoder::new(1, 1);
@@ -225,6 +240,7 @@ mod tests {
assert_eq!(&bytes[18..20], &291u16.to_le_bytes());
}
#[cfg(feature = "sensor-telemetry")]
#[test]
fn optical_flow_snapshot_uses_the_optical_flow_message() {
let mut encoder = MavlinkEncoder::new(1, 1);
libs/kaptan-mavlink/src/receiver.rs
+80 -54
View File
@@ -4,9 +4,9 @@ use crate::{MavlinkFrame, message};
/// Bounded MAVLink 2 receiver for a board's serial-byte interrupt.
///
/// Boards pass UART bytes to [`Self::receive_byte`] and take complete,
/// CRC-valid frames from [`Self::take_received`]. The API never decodes or
/// dispatches received message payloads.
/// Boards pass UART bytes to [`Self::receive_byte`] and take complete frames
/// from [`Self::take_received`]. Production has no signing verifier or replay
/// state, so it never exposes an inbound MAVLink frame as an authority record.
pub struct MavlinkReceiver<const N: usize> {
frame: MavlinkFrame,
received: usize,
@@ -23,7 +23,12 @@ impl<const N: usize> Default for MavlinkReceiver<N> {
}
impl<const N: usize> MavlinkReceiver<N> {
/// Creates a receiver with space for `N` complete MAVLink frames.
/// Creates a production receiver with space for `N` complete frames.
///
/// TODO(signing): accept a frame only after verified MAVLink-v2 signing,
/// configured sender/target binding, persistent anti-replay state, and
/// provisioning/rotation policy exist together. A V2 CRC only detects
/// corruption; it is not authentication.
pub const fn new() -> Self {
Self {
frame: MavlinkFrame::new(),
@@ -38,8 +43,10 @@ impl<const N: usize> MavlinkReceiver<N> {
/// Receives one UART byte and returns whether it queued a complete frame.
///
/// Bytes outside MAVLink 2 frames and frames with invalid CRCs are
/// discarded. If the completed-frame queue is full, the new frame is
/// dropped without displacing an earlier received frame.
/// discarded. Signed frames are also discarded because this receiver has
/// no verifier or replay state. Until the TODO(signing) work is complete,
/// unsigned frames are deliberately not promoted to received records
/// either.
pub fn receive_byte(&mut self, byte: u8) -> bool {
if self.received == 0 {
if byte == 0xfd {
@@ -54,23 +61,22 @@ impl<const N: usize> MavlinkReceiver<N> {
if self.received == 10 {
let header = self.frame.as_slice();
let incompatibility_flags = header[2];
if incompatibility_flags & !1 != 0 {
// MAVLink's bit 0 means the packet carries a signature. Reject all
// nonzero incompatibility flags until a reviewed signature verifier
// and persistent replay policy exist.
if header[2] != 0 {
self.reset();
return false;
}
self.expected = 1 + 9 + usize::from(header[1]) + 2;
if incompatibility_flags & 1 != 0 {
self.expected += 13;
}
}
if self.expected != 0 && self.received == self.expected {
let frame = core::mem::replace(&mut self.frame, MavlinkFrame::new());
self.received = 0;
self.expected = 0;
if frame.has_valid_crc::<message::MavMessage>() {
return self.push(frame);
if !frame.has_valid_crc::<message::MavMessage>() {
return false;
}
}
@@ -85,6 +91,9 @@ impl<const N: usize> MavlinkReceiver<N> {
}
/// Takes the oldest complete received frame, if one is available.
///
/// The production receiver returns no frame until signing verification is
/// implemented, but retaining this API keeps dispatch ownership explicit.
pub fn take_received(&mut self) -> Option<MavlinkFrame> {
if self.length == 0 {
return None;
@@ -95,14 +104,9 @@ impl<const N: usize> MavlinkReceiver<N> {
frame
}
fn push(&mut self, frame: MavlinkFrame) -> bool {
if N == 0 || self.length == N {
return false;
}
let index = (self.front + self.length) % N;
self.frames[index] = Some(frame);
self.length += 1;
true
/// Returns whether at least one complete frame awaits dispatch.
pub const fn has_received(&self) -> bool {
self.length != 0
}
}
@@ -131,23 +135,64 @@ mod tests {
frame
}
#[test]
fn returns_complete_crc_valid_frames_unchanged() {
let expected = heartbeat(0);
let mut receiver = MavlinkReceiver::<1>::new();
for byte in expected.raw_bytes() {
receiver.receive_byte(*byte);
}
assert_eq!(
receiver.take_received().unwrap().raw_bytes(),
expected.raw_bytes()
fn heartbeat_v1(sequence: u8) -> mavlink::MAVLinkV1MessageRaw {
let mut frame = mavlink::MAVLinkV1MessageRaw::new();
frame.serialize_message_data(
message::MavHeader {
sequence,
system_id: 42,
component_id: 7,
},
&message::HEARTBEAT_DATA {
custom_mode: 0,
mavtype: message::MavType::MAV_TYPE_QUADROTOR,
autopilot: message::MavAutopilot::MAV_AUTOPILOT_GENERIC,
base_mode: message::MavModeFlag::empty(),
system_status: message::MavState::MAV_STATE_STANDBY,
mavlink_version: 3,
},
);
frame
}
#[test]
fn discards_frames_with_an_invalid_crc() {
fn discards_a_crc_valid_mavlink_v1_frame() {
let mut receiver = MavlinkReceiver::<1>::new();
let v1 = heartbeat_v1(3);
assert_eq!(v1.raw_bytes()[0], 0xfe);
for byte in v1.raw_bytes() {
assert!(!receiver.receive_byte(*byte));
}
assert_eq!(receiver.take_received(), None);
}
#[test]
fn production_rejects_unsigned_frames_without_a_signing_verifier() {
let frame = heartbeat(0);
let mut receiver = MavlinkReceiver::<1>::new();
for byte in frame.raw_bytes() {
assert!(!receiver.receive_byte(*byte));
}
assert!(!receiver.has_received());
assert_eq!(receiver.take_received(), None);
}
#[test]
fn rejects_signed_v2_before_accepting_any_payload() {
let mut receiver = MavlinkReceiver::<1>::new();
// The first ten bytes contain the complete MAVLink 2 header. This
// frame advertises a signature (incompatibility bit 0), so rejection
// happens before its payload can be considered.
for byte in [0xfd, 0, 1, 0, 0, 1, 1, 0, 0, 0] {
assert!(!receiver.receive_byte(byte));
}
assert!(!receiver.has_received());
}
#[test]
fn a_crc_error_cannot_create_a_received_record() {
let frame = heartbeat(0);
let mut bytes = frame.into_inner();
bytes[10] ^= 1;
@@ -157,25 +202,6 @@ mod tests {
for byte in corrupt.raw_bytes() {
assert!(!receiver.receive_byte(*byte));
}
}
#[test]
fn preserves_arrival_order() {
let first = heartbeat(0);
let second = heartbeat(1);
let mut receiver = MavlinkReceiver::<2>::new();
for byte in first.raw_bytes().iter().chain(second.raw_bytes()) {
receiver.receive_byte(*byte);
}
assert_eq!(
receiver.take_received().unwrap().raw_bytes(),
first.raw_bytes()
);
assert_eq!(
receiver.take_received().unwrap().raw_bytes(),
second.raw_bytes()
);
assert!(!receiver.has_received());
}
}
libs/kaptan-mavlink/src/transmitter.rs
+163 -11
View File
@@ -54,20 +54,66 @@ where
class: FrameClass,
frame: MavlinkFrame,
) -> Result<EnqueueResult, MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
let admission = self.transmit.enqueue(PendingFrame { class, frame });
self.start_next().map(|()| admission)
let admission = self.enqueue_frame(class, frame);
self.start_pending().map(|()| admission)
}
/// Admits a frame without attempting to start the DMA engine.
///
/// This split lifecycle is for hardware routes whose terminal DMA IRQ can
/// arrive a few bus cycles before the stream's `EN` bit reads clear. Such
/// a route may acknowledge a completion, retain the selected frame, and
/// retry [`Self::start_pending`] from its next bounded service turn rather
/// than treating that transient `Busy` observation as a transport fault.
/// Callers still own the policy for a queue-full result.
pub fn enqueue_frame(&mut self, class: FrameClass, frame: MavlinkFrame) -> EnqueueResult {
self.transmit.enqueue(PendingFrame { class, frame })
}
/// Attempts to start one pending DMA frame.
///
/// A rejected engine start leaves the exact frame reserved for a later
/// retry. This method therefore does not discard or overwrite DMA source
/// storage on a transient hardware-not-ready result.
pub fn start_pending(
&mut self,
) -> Result<(), MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
self.start_next()
}
/// Releases a terminal DMA frame and opportunistically starts the next.
pub fn on_dma_complete(&mut self, completion: TxCompletion) {
///
/// The returned class is the frame that reached a terminal DMA result,
/// not a class selected from newly pending work. A policy can therefore
/// distinguish completion classes without inferring them from newly
/// pending work.
pub fn on_dma_complete(
&mut self,
completion: TxCompletion,
) -> Result<Option<FrameClass>, MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
let completed = self.complete_dma(completion)?;
if completion == TxCompletion::Complete {
self.start_pending()?;
}
Ok(completed)
}
/// Releases the active frame after a terminal DMA result without starting
/// another one.
///
/// Most routes use [`Self::on_dma_complete`], which preserves legacy
/// immediate restart behavior. A route whose hardware exposes terminal
/// status before its enable bit is observably low uses this method and
/// calls [`Self::start_pending`] from a later bounded service turn.
pub fn complete_dma(
&mut self,
completion: TxCompletion,
) -> Result<Option<FrameClass>, MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
match completion {
TxCompletion::NotPending => {}
TxCompletion::Complete => {
let _ = self.transmit.complete();
let _ = self.start_next();
}
TxCompletion::NotPending => Ok(None),
TxCompletion::Complete => Ok(self.transmit.complete().map(|frame| frame.class)),
TxCompletion::TransferFault | TxCompletion::Halted => {
let _ = self.transmit.fail();
Ok(self.transmit.fail().map(|frame| frame.class))
}
}
}
@@ -89,7 +135,7 @@ where
#[cfg(test)]
mod tests {
use super::{EnqueueResult, FrameClass, MavlinkTransmitter};
use super::{EnqueueResult, FrameClass, MavlinkTransmitFault, MavlinkTransmitter};
use crate::{encoder::MavlinkEncoder, message::MavState};
use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion};
@@ -114,6 +160,44 @@ mod tests {
}
}
struct RecordingEngine {
started_sequences: [u8; 4],
starts: usize,
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
enum RetryError {
Busy,
}
struct BusyOnceEngine {
busy_once: bool,
starts: usize,
}
impl DmaTxEngine for BusyOnceEngine {
type StartError = RetryError;
fn start(&mut self, _: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> {
if self.busy_once {
self.busy_once = false;
return Err(RetryError::Busy);
}
self.starts += 1;
Ok(())
}
}
impl DmaTxEngine for RecordingEngine {
type StartError = core::convert::Infallible;
fn start(&mut self, buffer: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> {
self.started_sequences[self.starts] = buffer.bytes()[4];
self.starts += 1;
Ok(())
}
}
#[test]
fn transfer_fault_drops_only_the_active_frame() {
let mut encoder = MavlinkEncoder::new(1, 1);
@@ -132,7 +216,9 @@ mod tests {
.unwrap();
assert!(transmitter.transmit.is_active());
transmitter.on_dma_complete(TxCompletion::TransferFault);
transmitter
.on_dma_complete(TxCompletion::TransferFault)
.unwrap();
assert!(!transmitter.transmit.is_active());
}
@@ -169,4 +255,70 @@ mod tests {
EnqueueResult::CriticalQueueFull
);
}
#[test]
fn terminal_completion_can_defer_a_transient_engine_busy_without_losing_frame_order() {
let mut encoder = MavlinkEncoder::new(1, 1);
let mut transmitter = MavlinkTransmitter::<Memory, BusyOnceEngine, 2>::new(
Memory,
BusyOnceEngine {
busy_once: false,
starts: 0,
},
);
let state = MavState::MAV_STATE_STANDBY;
transmitter.enqueue_frame(FrameClass::Critical, encoder.heartbeat(state, false));
transmitter.start_pending().unwrap();
assert_eq!(transmitter.engine.starts, 1);
transmitter.enqueue_frame(FrameClass::Critical, encoder.heartbeat(state, false));
// Model the STM32 case: TCIF is acknowledged but CR.EN still reports
// busy for the first attempt to start the successor. `complete_dma`
// deliberately does not start it in the terminal IRQ path.
assert_eq!(
transmitter.complete_dma(TxCompletion::Complete).unwrap(),
Some(FrameClass::Critical)
);
assert_eq!(transmitter.engine.starts, 1);
transmitter.engine.busy_once = true;
assert!(matches!(
transmitter.start_pending(),
Err(MavlinkTransmitFault::Engine(RetryError::Busy))
));
assert!(!transmitter.transmit.is_active());
transmitter.start_pending().unwrap();
assert!(transmitter.transmit.is_active());
assert_eq!(transmitter.engine.starts, 2);
}
#[test]
fn completion_retry_starts_pending_best_effort_before_later_critical_frame() {
let mut encoder = MavlinkEncoder::new(1, 1);
let mut transmitter = MavlinkTransmitter::<Memory, RecordingEngine, 3>::new(
Memory,
RecordingEngine {
started_sequences: [0; 4],
starts: 0,
},
);
let state = MavState::MAV_STATE_STANDBY;
// A critical frame is active. A best-effort sample arrives behind it
// before DMA completion.
transmitter.enqueue_frame(FrameClass::Critical, encoder.heartbeat(state, false));
transmitter.start_pending().unwrap();
transmitter.enqueue_frame(FrameClass::BestEffort, encoder.heartbeat(state, false));
// The completion worker performs its bounded retry before the next
// periodic sender can enqueue fresh critical traffic. The existing
// best-effort frame therefore cannot starve behind later traffic.
transmitter.complete_dma(TxCompletion::Complete).unwrap();
transmitter.start_pending().unwrap();
transmitter.enqueue_frame(FrameClass::Critical, encoder.heartbeat(state, false));
assert_eq!(transmitter.engine.starts, 2);
assert_eq!(&transmitter.engine.started_sequences[..2], &[0, 1]);
}
}