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

feat(mavlink): adopt the kaptan-native dialect

Browse Source
This commit is contained in:
Onur Özkan committed 2026-08-24 00:56:06 +03:00
1 parent 1c90870634
commit 5f91465e66
87 files changed
+7733 -6850
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

firmware/libs/kaptan-mavlink/src/commands/arm.rs
-102
View File
@@ -1,102 +0,0 @@
//! Narrow MAVLink arm/disarm command decoding.
//!
//! This module deliberately accepts no actuator-value or motor-test command.
//! A board must pass accepted requests to its single actuator-state owner; this
//! decoder has no access to output hardware.
use mavlink::Message;
use crate::{MavlinkFrame, message};
/// A requested change to the vehicle's arming state.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum ArmRequest {
/// Request the armed-idle state.
Arm,
/// Request electrically inactive outputs.
Disarm,
}
/// Decodes a targeted MAVLink `MAV_CMD_COMPONENT_ARM_DISARM` command.
///
/// Only `param1` values exactly equal to `0.0` (disarm) or `1.0` (arm) are
/// accepted. Broadcast targets are intentionally rejected: an actuator command
/// must name this vehicle and its component explicitly.
pub fn decode_arm_request(
frame: &MavlinkFrame,
system_id: u8,
component_id: u8,
) -> Option<ArmRequest> {
let message = message::MavMessage::parse(
mavlink::MavlinkVersion::V2,
frame.message_id(),
frame.payload(),
)
.ok()?;
let message::MavMessage::COMMAND_LONG(command) = message else {
return None;
};
if command.command != message::MavCmd::MAV_CMD_COMPONENT_ARM_DISARM
|| command.target_system != system_id
|| command.target_component != component_id
{
return None;
}
match command.param1 {
0.0 => Some(ArmRequest::Disarm),
1.0 => Some(ArmRequest::Arm),
_ => None,
}
}
#[cfg(test)]
mod tests {
use super::{ArmRequest, decode_arm_request};
use crate::{MavlinkFrame, message};
fn command(target_system: u8, target_component: u8, value: f32) -> MavlinkFrame {
let mut frame = MavlinkFrame::new();
frame.serialize_message_data(
message::MavHeader {
sequence: 0,
system_id: 42,
component_id: 7,
},
&message::COMMAND_LONG_DATA {
param1: value,
param2: 0.0,
param3: 0.0,
param4: 0.0,
param5: 0.0,
param6: 0.0,
param7: 0.0,
command: message::MavCmd::MAV_CMD_COMPONENT_ARM_DISARM,
target_system,
target_component,
confirmation: 0,
},
);
frame
}
#[test]
fn accepts_explicit_targeted_arm_and_disarm() {
assert_eq!(
decode_arm_request(&command(1, 1, 1.0), 1, 1),
Some(ArmRequest::Arm)
);
assert_eq!(
decode_arm_request(&command(1, 1, 0.0), 1, 1),
Some(ArmRequest::Disarm)
);
}
#[test]
fn rejects_broadcast_wrong_target_and_invalid_parameter() {
assert_eq!(decode_arm_request(&command(0, 0, 1.0), 1, 1), None);
assert_eq!(decode_arm_request(&command(2, 1, 1.0), 1, 1), None);
assert_eq!(decode_arm_request(&command(1, 1, 0.5), 1, 1), None);
}
}
firmware/libs/kaptan-mavlink/src/commands/mod.rs
-4
View File
@@ -1,4 +0,0 @@
//! MAVLink command decoders.
/// Targeted arm/disarm command decoding.
pub mod arm;
firmware/libs/kaptan-mavlink/src/encoder.rs
+157 -152
View File
@@ -6,23 +6,14 @@ use kaptan_sensor::imu::ImuSnapshot;
use kaptan_sensor::optical_flow::OpticalFlowSnapshot;
#[cfg(feature = "sensor-telemetry")]
use kaptan_sensor::range::RangeSnapshot;
use mavlink::MessageData;
use mavlink_core::{MavHeader, 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.
pub sensor_id: u8,
/// Sensor mounting orientation.
pub orientation: message::MavSensorOrientation,
/// Lower valid measurement bound, in centimetres.
pub minimum_distance_centimetres: u16,
/// Upper valid measurement bound, in centimetres.
pub maximum_distance_centimetres: u16,
}
use crate::generated::kaptan::{IMU_DATA, OPTICAL_FLOW_DATA, RANGE_DATA};
use crate::{
MavlinkFrame,
generated::kaptan::{HEARTBEAT_DATA, HeartbeatState, MavMessage},
};
/// Serializes FC-domain data with MAVLink sender identity and sequence state.
pub struct MavlinkEncoder {
@@ -34,6 +25,10 @@ pub struct MavlinkEncoder {
impl MavlinkEncoder {
/// Creates an encoder beginning with MAVLink packet sequence zero.
pub const fn new(system_id: u8, component_id: u8) -> Self {
assert!(
system_id != 0 && component_id != 0,
"MAVLink source must be exact"
);
Self {
system_id,
component_id,
@@ -41,124 +36,78 @@ impl MavlinkEncoder {
}
}
fn serialize<M: MessageData>(&mut self, message: &M) -> MavlinkFrame {
let mut raw = MavlinkFrame::new();
raw.serialize_message_data(
crate::message::MavHeader {
sequence: self.next_sequence,
system_id: self.system_id,
component_id: self.component_id,
},
message,
);
fn header(&mut self) -> MavHeader {
let header = MavHeader {
sequence: self.next_sequence,
system_id: self.system_id,
component_id: self.component_id,
};
self.next_sequence = self.next_sequence.wrapping_add(1);
header
}
/// Encodes one concrete generated Kaptan record through this endpoint's
/// sole header-sequence owner.
pub fn encode<M: MessageData<Message = MavMessage>>(&mut self, data: &M) -> MavlinkFrame {
let mut raw = MavlinkFrame::new();
raw.serialize_message_data(self.header(), data);
raw
}
/// Encodes one generated MAVLink data record through this endpoint's sole
/// header-sequence owner. Runtime-log/config adapters return data records
/// and must use this method rather than create a competing encoder.
pub fn custom<M: MessageData>(&mut self, message: &M) -> MavlinkFrame {
self.serialize(message)
}
/// 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_ONBOARD_CONTROLLER,
autopilot: message::MavAutopilot::MAV_AUTOPILOT_INVALID,
base_mode: if armed {
message::MavModeFlag::MAV_MODE_FLAG_SAFETY_ARMED
} else {
message::MavModeFlag::empty()
},
system_status,
mavlink_version: 3,
/// Encodes the current initialized, output-inhibited heartbeat.
pub fn heartbeat(&mut self) -> MavlinkFrame {
let heartbeat = HEARTBEAT_DATA {
state: HeartbeatState::HEARTBEAT_STATE_INITIALIZED_OUTPUTS_INHIBITED,
};
self.serialize(&heartbeat)
self.encode(&heartbeat)
}
/// Encodes a standard `RAW_IMU` frame.
/// Encodes one native Kaptan inertial observation.
#[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(),
xacc: snapshot.acceleration_milligravity[0],
yacc: snapshot.acceleration_milligravity[1],
zacc: snapshot.acceleration_milligravity[2],
xgyro: snapshot.angular_rate_milliradians_per_second[0],
ygyro: snapshot.angular_rate_milliradians_per_second[1],
zgyro: snapshot.angular_rate_milliradians_per_second[2],
xmag: 0,
ymag: 0,
zmag: 0,
pub fn imu_snapshot(&mut self, snapshot: &ImuSnapshot) -> MavlinkFrame {
let imu = IMU_DATA {
time_us: snapshot.timestamp.as_microseconds(),
acceleration_milligravity: snapshot.acceleration_milligravity,
angular_rate_milliradians_per_second: snapshot.angular_rate_milliradians_per_second,
temperature_centidegrees_celsius: snapshot.temperature_centidegrees_celsius,
};
self.serialize(&raw_imu)
self.encode(&imu)
}
/// Encodes a standard `DISTANCE_SENSOR` frame for a laser rangefinder.
///
/// MAVLink represents distance in centimetres; the sensor domain keeps
/// millimetres so this conversion happens only at the protocol boundary.
/// Encodes one native Kaptan downward-range observation.
#[cfg(feature = "sensor-telemetry")]
pub fn distance_sensor_snapshot(
&mut self,
snapshot: &RangeSnapshot,
config: DistanceSensorConfig,
) -> MavlinkFrame {
let current_distance = (snapshot.distance_millimetres / 10).clamp(
u32::from(config.minimum_distance_centimetres),
u32::from(config.maximum_distance_centimetres),
) as u16;
let distance_sensor = message::DISTANCE_SENSOR_DATA {
time_boot_ms: (snapshot.timestamp.as_microseconds() / 1_000) as u32,
min_distance: config.minimum_distance_centimetres,
max_distance: config.maximum_distance_centimetres,
current_distance,
mavtype: message::MavDistanceSensor::MAV_DISTANCE_SENSOR_LASER,
id: config.sensor_id,
orientation: config.orientation,
covariance: u8::MAX,
pub fn range_snapshot(&mut self, snapshot: &RangeSnapshot) -> MavlinkFrame {
let range = RANGE_DATA {
time_us: snapshot.timestamp.as_microseconds(),
distance_millimetres: snapshot.distance_millimetres,
};
self.serialize(&distance_sensor)
self.encode(&range)
}
/// Encodes a standard `OPTICAL_FLOW` frame from a ground-referenced flow
/// observation.
/// Encodes one native Kaptan ground-referenced optical-flow observation.
#[cfg(feature = "sensor-telemetry")]
pub fn optical_flow_snapshot(
&mut self,
snapshot: &OpticalFlowSnapshot,
sensor_id: u8,
) -> MavlinkFrame {
let optical_flow = message::OPTICAL_FLOW_DATA {
time_usec: snapshot.timestamp.as_microseconds(),
sensor_id,
// Kaptan's sensor domain contains ground-referenced velocity,
// not unscaled image-pixel displacement.
flow_x: 0,
flow_y: 0,
flow_comp_m_x: snapshot.velocity_millimetres_per_second[0] as f32 / 1_000.0,
flow_comp_m_y: snapshot.velocity_millimetres_per_second[1] as f32 / 1_000.0,
pub fn optical_flow_snapshot(&mut self, snapshot: &OpticalFlowSnapshot) -> MavlinkFrame {
let optical_flow = OPTICAL_FLOW_DATA {
time_us: snapshot.timestamp.as_microseconds(),
velocity_millimetres_per_second: snapshot.velocity_millimetres_per_second,
ground_distance_millimetres: snapshot.ground_distance_millimetres,
quality: snapshot.quality,
ground_distance: snapshot.ground_distance_millimetres as f32 / 1_000.0,
};
self.serialize(&optical_flow)
self.encode(&optical_flow)
}
}
#[cfg(test)]
mod tests {
#[cfg(feature = "sensor-telemetry")]
use super::DistanceSensorConfig;
use super::MavlinkEncoder;
use crate::message::MavState;
use crate::{
MavlinkFrame,
generated::kaptan::{
ERROR_DATA, FILE_CONTROL_DATA, FILE_DATA_DATA, HEARTBEAT_DATA, HeartbeatState,
IMU_DATA, LOG_DATA, LogFlags, LogLevel, MavMessage, OPTICAL_FLOW_DATA, RANGE_DATA,
},
receiver::IntegrityMavlinkReceiver,
};
#[cfg(feature = "sensor-telemetry")]
use kaptan_sensor::imu::ImuSnapshot;
#[cfg(feature = "sensor-telemetry")]
@@ -168,11 +117,20 @@ mod tests {
#[cfg(feature = "sensor-telemetry")]
use kaptan_time::Timestamp;
fn parse_test_message(frame: &MavlinkFrame) -> Option<MavMessage> {
let mut receiver = IntegrityMavlinkReceiver::<1>::new();
for byte in frame.raw_bytes() {
receiver.receive_byte(*byte);
}
receiver.take_received()?.decode()
}
#[test]
fn heartbeat_has_the_configured_sender_and_sequence() {
let mut encoder = MavlinkEncoder::new(42, 7);
let frame = encoder.heartbeat(MavState::MAV_STATE_STANDBY, false);
let bytes = frame.raw_bytes();
let first = encoder.heartbeat();
let second = encoder.heartbeat();
let bytes = first.raw_bytes();
assert_eq!(bytes[0], 0xFD);
assert_eq!(bytes[2], 0);
@@ -180,15 +138,14 @@ mod tests {
assert_eq!(bytes[4], 0);
assert_eq!(bytes[5], 42);
assert_eq!(bytes[6], 7);
assert_eq!(&bytes[7..10], &[0, 0, 0]);
}
#[test]
fn heartbeat_preserves_the_requested_health_state() {
let mut encoder = MavlinkEncoder::new(1, 1);
let frame = encoder.heartbeat(MavState::MAV_STATE_CRITICAL, false);
assert_eq!(frame.raw_bytes()[17], MavState::MAV_STATE_CRITICAL as u8);
assert_eq!(&bytes[7..10], &[0x53, 0xc7, 0]);
assert_eq!(second.raw_bytes()[4], 1);
assert!(matches!(
parse_test_message(&first),
Some(MavMessage::HEARTBEAT(data))
if data.state
== HeartbeatState::HEARTBEAT_STATE_INITIALIZED_OUTPUTS_INHIBITED
));
}
#[cfg(feature = "sensor-telemetry")]
@@ -202,45 +159,36 @@ mod tests {
2_500,
);
let frame = encoder.raw_imu_snapshot(&snapshot);
let frame = encoder.imu_snapshot(&snapshot);
assert_eq!(&frame.raw_bytes()[7..10], &[27, 0, 0]);
}
#[test]
fn armed_heartbeat_sets_the_standard_armed_flag() {
let mut encoder = MavlinkEncoder::new(1, 1);
let frame = encoder.heartbeat(MavState::MAV_STATE_ACTIVE, true);
assert_ne!(frame.raw_bytes()[16] & 0x80, 0);
assert!(matches!(
parse_test_message(&frame),
Some(MavMessage::IMU(data))
if data.time_us == 123
&& data.acceleration_milligravity == [1, 2, 3]
&& data.angular_rate_milliradians_per_second == [4, 5, 6]
&& data.temperature_centidegrees_celsius == 2_500
));
}
#[cfg(feature = "sensor-telemetry")]
#[test]
fn range_snapshot_uses_distance_sensor_centimetres() {
fn range_snapshot_preserves_native_time_and_distance() {
let mut encoder = MavlinkEncoder::new(1, 1);
let snapshot = RangeSnapshot::new(Timestamp::from_microseconds(123_000), 2_910);
let frame = encoder.distance_sensor_snapshot(
&snapshot,
DistanceSensorConfig {
sensor_id: 0,
orientation: crate::message::MavSensorOrientation::MAV_SENSOR_ROTATION_PITCH_270,
minimum_distance_centimetres: 20,
maximum_distance_centimetres: 4_000,
},
);
let bytes = frame.raw_bytes();
let frame = encoder.range_snapshot(&snapshot);
assert_eq!(&bytes[7..10], &[132, 0, 0]);
// MAVLink payload starts after the ten-byte MAVLink 2 header. The
// current distance field follows time, minimum, and maximum range.
assert_eq!(&bytes[18..20], &291u16.to_le_bytes());
assert!(matches!(
parse_test_message(&frame),
Some(MavMessage::RANGE(data))
if data.time_us == 123_000 && data.distance_millimetres == 2_910
));
}
#[cfg(feature = "sensor-telemetry")]
#[test]
fn optical_flow_snapshot_uses_the_optical_flow_message() {
fn optical_flow_snapshot_preserves_native_integer_observation() {
let mut encoder = MavlinkEncoder::new(1, 1);
let snapshot = OpticalFlowSnapshot::new(
Timestamp::from_microseconds(123_000),
@@ -249,13 +197,70 @@ mod tests {
200,
);
let frame = encoder.optical_flow_snapshot(&snapshot, 3);
let bytes = frame.raw_bytes();
let frame = encoder.optical_flow_snapshot(&snapshot);
assert_eq!(&bytes[7..10], &[100, 0, 0]);
// Optical-flow payload begins after the MAVLink 2 header.
assert_eq!(&bytes[10..18], &123_000_u64.to_le_bytes());
assert_eq!(&bytes[18..22], &1.0_f32.to_le_bytes());
assert_eq!(&bytes[22..26], &(-0.5_f32).to_le_bytes());
assert!(matches!(
parse_test_message(&frame),
Some(MavMessage::OPTICAL_FLOW(data))
if data.time_us == 123_000
&& data.velocity_millimetres_per_second == [1_000, -500]
&& data.ground_distance_millimetres == 2_000
&& data.quality == 200
));
}
#[test]
fn dialect_wire_contract_is_stable() {
use mavlink_core::MessageData;
assert_eq!(HEARTBEAT_DATA::NAME, "HEARTBEAT");
assert_eq!(HEARTBEAT_DATA::ID, 51_027);
assert_eq!(HEARTBEAT_DATA::EXTRA_CRC, 161);
assert_eq!(HEARTBEAT_DATA::ENCODED_LEN, 1);
assert_eq!(IMU_DATA::NAME, "IMU");
assert_eq!(IMU_DATA::ID, 51_028);
assert_eq!(IMU_DATA::EXTRA_CRC, 194);
assert_eq!(IMU_DATA::ENCODED_LEN, 22);
assert_eq!(RANGE_DATA::NAME, "RANGE");
assert_eq!(RANGE_DATA::ID, 51_029);
assert_eq!(RANGE_DATA::EXTRA_CRC, 44);
assert_eq!(RANGE_DATA::ENCODED_LEN, 12);
assert_eq!(OPTICAL_FLOW_DATA::NAME, "OPTICAL_FLOW");
assert_eq!(OPTICAL_FLOW_DATA::ID, 51_030);
assert_eq!(OPTICAL_FLOW_DATA::EXTRA_CRC, 202);
assert_eq!(OPTICAL_FLOW_DATA::ENCODED_LEN, 21);
assert_eq!(LOG_DATA::NAME, "LOG");
assert_eq!(LOG_DATA::ID, 51_020);
assert_eq!(LOG_DATA::EXTRA_CRC, 120);
assert_eq!(LOG_DATA::ENCODED_LEN, 163);
assert_eq!(ERROR_DATA::NAME, "ERROR");
assert_eq!(ERROR_DATA::ID, 51_021);
assert_eq!(ERROR_DATA::EXTRA_CRC, 134);
assert_eq!(ERROR_DATA::ENCODED_LEN, 98);
assert_eq!(FILE_CONTROL_DATA::NAME, "FILE_CONTROL");
assert_eq!(FILE_CONTROL_DATA::ID, 51_025);
assert_eq!(FILE_CONTROL_DATA::EXTRA_CRC, 161);
assert_eq!(FILE_CONTROL_DATA::ENCODED_LEN, 83);
assert_eq!(FILE_DATA_DATA::NAME, "FILE_DATA");
assert_eq!(FILE_DATA_DATA::ID, 51_026);
assert_eq!(FILE_DATA_DATA::EXTRA_CRC, 20);
assert_eq!(FILE_DATA_DATA::ENCODED_LEN, 255);
assert_eq!(
MavMessage::all_ids(),
&[
51_020, 51_021, 51_025, 51_026, 51_027, 51_028, 51_029, 51_030
]
);
assert_eq!(
HeartbeatState::HEARTBEAT_STATE_INITIALIZED_OUTPUTS_INHIBITED as u8,
1
);
assert_eq!(LogLevel::LOG_LEVEL_OFF as u8, 0);
assert_eq!(LogLevel::LOG_LEVEL_TRACE as u8, 1);
assert_eq!(LogLevel::LOG_LEVEL_DEBUG as u8, 2);
assert_eq!(LogLevel::LOG_LEVEL_INFO as u8, 3);
assert_eq!(LogLevel::LOG_LEVEL_WARN as u8, 4);
assert_eq!(LogLevel::LOG_LEVEL_ERROR as u8, 5);
assert_eq!(LogFlags::LOG_FLAG_TRUNCATED.bits(), 1);
}
}
firmware/libs/kaptan-mavlink/src/lib.rs
+14 -14
View File
@@ -2,8 +2,6 @@
#![no_std]
/// MAVLink command decoders.
pub mod commands;
/// MAVLink frame encoding API for boards.
pub mod encoder;
/// Serial-byte MAVLink receive API for boards.
@@ -12,18 +10,20 @@ pub mod receiver;
pub mod transmitter;
/// An owned MAVLink 2 frame suitable for UART transmission.
pub type MavlinkFrame = message::MAVLinkV2MessageRaw;
/// MAVLink framing version used when parsing an integrity-checked payload.
pub use mavlink_core::MavlinkVersion;
pub type MavlinkFrame = mavlink_core::MAVLinkV2MessageRaw;
/// Supported MAVLink message types for Kaptan.
pub mod message {
pub use mavlink::MAVLinkV2MessageRaw;
pub use mavlink::MavHeader;
pub use mavlink::Message;
pub use mavlink::MessageData;
pub use mavlink::dialects::common::*;
/// Generated wire types for Kaptan's sole MAVLink dialect.
pub mod generated {
include!(concat!(env!("OUT_DIR"), "/kaptan_module.rs"));
}
#[cfg(kaptan_has_messages)]
include!(concat!(env!("OUT_DIR"), "/mod.rs"));
fn decode_message(frame: &MavlinkFrame) -> Option<generated::kaptan::MavMessage> {
use mavlink_core::Message;
generated::kaptan::MavMessage::parse(
mavlink_core::MavlinkVersion::V2,
frame.message_id(),
frame.payload(),
)
.ok()
}
firmware/libs/kaptan-mavlink/src/receiver.rs
+163 -46
View File
@@ -1,6 +1,36 @@
//! MAVLink serial-byte receive API.
use crate::{MavlinkFrame, message};
use crate::{MavlinkFrame, decode_message, generated::kaptan::MavMessage};
/// An unsigned MAVLink 2 frame whose ID and CRC match the Kaptan dialect.
///
/// The payload is deliberately not decoded during byte admission. Consumers
/// decode it once after taking the frame from the bounded receive queue.
#[repr(transparent)]
pub struct IntegrityCheckedFrame(MavlinkFrame);
const _: [(); core::mem::size_of::<MavlinkFrame>()] =
[(); core::mem::size_of::<IntegrityCheckedFrame>()];
const _: [(); core::mem::align_of::<MavlinkFrame>()] =
[(); core::mem::align_of::<IntegrityCheckedFrame>()];
impl IntegrityCheckedFrame {
fn try_new(frame: MavlinkFrame) -> Option<Self> {
let message_id = frame.message_id();
(MavMessage::all_ids().contains(&message_id) && frame.has_valid_crc::<MavMessage>())
.then_some(Self(frame))
}
/// Returns the admitted raw frame without copying it.
pub const fn raw(&self) -> &MavlinkFrame {
&self.0
}
/// Decodes the admitted Kaptan payload without repeating its CRC check.
pub fn decode(&self) -> Option<MavMessage> {
decode_message(&self.0)
}
}
struct IntegrityFramer {
frame: MavlinkFrame,
@@ -17,7 +47,7 @@ impl IntegrityFramer {
}
}
fn receive_byte(&mut self, byte: u8) -> Option<MavlinkFrame> {
fn receive_byte(&mut self, byte: u8) -> Option<IntegrityCheckedFrame> {
if self.received == 0 {
if byte == 0xfd {
self.frame.as_mut_slice()[0] = byte;
@@ -48,7 +78,7 @@ impl IntegrityFramer {
let frame = core::mem::replace(&mut self.frame, MavlinkFrame::new());
self.received = 0;
self.expected = 0;
has_valid_known_crc(&frame).then_some(frame)
IntegrityCheckedFrame::try_new(frame)
}
fn reset(&mut self) {
@@ -62,10 +92,10 @@ impl IntegrityFramer {
///
/// It rejects signed, incompatible, corrupt, and MAVLink 1 frames, but an
/// accepted frame is still unauthenticated. The composition decides whether
/// and how a received command may be admitted.
/// and how a received request may be admitted.
pub struct IntegrityMavlinkReceiver<const N: usize> {
framer: IntegrityFramer,
frames: [Option<MavlinkFrame>; N],
frames: [Option<IntegrityCheckedFrame>; N],
front: usize,
length: usize,
}
@@ -76,7 +106,7 @@ impl<const N: usize> IntegrityMavlinkReceiver<N> {
assert!(N != 0, "integrity receiver capacity must be nonzero");
Self {
framer: IntegrityFramer::new(),
frames: [None; N],
frames: [const { None }; N],
front: 0,
length: 0,
}
@@ -99,13 +129,13 @@ impl<const N: usize> IntegrityMavlinkReceiver<N> {
/// Discards partial framing state and all queued unauthenticated frames.
pub fn reset(&mut self) {
self.framer.reset();
self.frames = [None; N];
self.frames = [const { None }; N];
self.front = 0;
self.length = 0;
}
/// Takes the oldest integrity-checked frame.
pub fn take_received(&mut self) -> Option<MavlinkFrame> {
pub fn take_received(&mut self) -> Option<IntegrityCheckedFrame> {
if self.length == 0 {
return None;
}
@@ -122,59 +152,107 @@ impl<const N: usize> Default for IntegrityMavlinkReceiver<N> {
}
}
fn has_valid_known_crc(frame: &MavlinkFrame) -> bool {
let common_valid = frame.has_valid_crc::<message::MavMessage>();
#[cfg(feature = "dialect-kaptan")]
{
common_valid || frame.has_valid_crc::<message::kaptan::MavMessage>()
}
#[cfg(not(feature = "dialect-kaptan"))]
{
common_valid
}
}
#[cfg(test)]
mod tests {
use super::IntegrityMavlinkReceiver;
use crate::{MavlinkFrame, message};
use super::{IntegrityCheckedFrame, IntegrityMavlinkReceiver};
use crate::{
MavlinkFrame,
generated::kaptan::{HEARTBEAT_DATA, HeartbeatState, MavMessage},
};
use mavlink_core::{
MAVLinkV1MessageRaw, MavHeader, MavlinkVersion, MessageData, error::ParserError,
};
struct ForeignHeartbeat;
impl MessageData for ForeignHeartbeat {
type Message = MavMessage;
const ID: u32 = 0;
const NAME: &'static str = "FOREIGN_HEARTBEAT";
const EXTRA_CRC: u8 = 50;
const ENCODED_LEN: usize = 9;
fn ser(&self, _: MavlinkVersion, payload: &mut [u8]) -> usize {
payload[..Self::ENCODED_LEN].copy_from_slice(&[0, 0, 0, 0, 0, 0, 0, 0, 3]);
Self::ENCODED_LEN
}
fn deser(_: MavlinkVersion, _: &[u8]) -> Result<Self, ParserError> {
Ok(Self)
}
}
struct MalformedKaptanHeartbeat;
impl MessageData for MalformedKaptanHeartbeat {
type Message = MavMessage;
const ID: u32 = HEARTBEAT_DATA::ID;
const NAME: &'static str = HEARTBEAT_DATA::NAME;
const EXTRA_CRC: u8 = HEARTBEAT_DATA::EXTRA_CRC;
const ENCODED_LEN: usize = HEARTBEAT_DATA::ENCODED_LEN;
fn ser(&self, _: MavlinkVersion, payload: &mut [u8]) -> usize {
payload[0] = 0;
Self::ENCODED_LEN
}
fn deser(_: MavlinkVersion, _: &[u8]) -> Result<Self, ParserError> {
Ok(Self)
}
}
fn heartbeat(sequence: u8) -> MavlinkFrame {
let mut frame = MavlinkFrame::new();
frame.serialize_message_data(
message::MavHeader {
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,
&HEARTBEAT_DATA {
state: HeartbeatState::HEARTBEAT_STATE_INITIALIZED_OUTPUTS_INHIBITED,
},
);
frame
}
fn heartbeat_v1(sequence: u8) -> mavlink::MAVLinkV1MessageRaw {
let mut frame = mavlink::MAVLinkV1MessageRaw::new();
fn foreign_heartbeat_v1(sequence: u8) -> MAVLinkV1MessageRaw {
let mut frame = MAVLinkV1MessageRaw::new();
frame.serialize_message_data(
message::MavHeader {
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,
&ForeignHeartbeat,
);
frame
}
fn foreign_heartbeat_v2(sequence: u8) -> MavlinkFrame {
let mut frame = MavlinkFrame::new();
frame.serialize_message_data(
MavHeader {
sequence,
system_id: 42,
component_id: 7,
},
&ForeignHeartbeat,
);
frame
}
fn malformed_kaptan_heartbeat(sequence: u8) -> MavlinkFrame {
let mut frame = MavlinkFrame::new();
frame.serialize_message_data(
MavHeader {
sequence,
system_id: 42,
component_id: 7,
},
&MalformedKaptanHeartbeat,
);
frame
}
@@ -182,14 +260,26 @@ mod tests {
#[test]
fn discards_a_crc_valid_mavlink_v1_frame() {
let mut receiver = IntegrityMavlinkReceiver::<1>::new();
let v1 = heartbeat_v1(3);
let v1 = foreign_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);
assert!(receiver.take_received().is_none());
}
#[test]
fn discards_a_crc_valid_foreign_dialect_frame() {
let foreign = foreign_heartbeat_v2(3);
let mut receiver = IntegrityMavlinkReceiver::<1>::new();
for byte in foreign.raw_bytes() {
assert!(!receiver.receive_byte(*byte));
}
assert!(receiver.take_received().is_none());
}
#[test]
@@ -199,8 +289,35 @@ mod tests {
for byte in frame.raw_bytes() {
receiver.receive_byte(*byte);
}
assert_eq!(receiver.take_received(), Some(frame));
assert_eq!(receiver.take_received(), None);
let received = receiver.take_received().expect("complete frame");
assert_eq!(received.raw(), &frame);
assert_eq!(received.decode(), crate::decode_message(&frame));
assert!(receiver.take_received().is_none());
}
#[test]
fn defers_payload_decoding_until_after_integrity_admission() {
let frame = malformed_kaptan_heartbeat(9);
let mut receiver = IntegrityMavlinkReceiver::<1>::new();
for byte in frame.raw_bytes() {
receiver.receive_byte(*byte);
}
let received = receiver.take_received().expect("CRC-valid Kaptan frame");
assert_eq!(received.raw(), &frame);
assert_eq!(received.decode(), None);
}
#[test]
fn integrity_wrapper_has_exactly_the_raw_frame_layout() {
assert_eq!(
core::mem::size_of::<IntegrityCheckedFrame>(),
core::mem::size_of::<MavlinkFrame>()
);
assert_eq!(
core::mem::align_of::<IntegrityCheckedFrame>(),
core::mem::align_of::<MavlinkFrame>()
);
}
#[test]
@@ -212,7 +329,7 @@ mod tests {
for byte in [0xfd, 0, 1, 0, 0, 1, 1, 0, 0, 0] {
assert!(!receiver.receive_byte(byte));
}
assert_eq!(receiver.take_received(), None);
assert!(receiver.take_received().is_none());
}
#[test]
@@ -226,6 +343,6 @@ mod tests {
for byte in corrupt.raw_bytes() {
assert!(!receiver.receive_byte(*byte));
}
assert_eq!(receiver.take_received(), None);
assert!(receiver.take_received().is_none());
}
}
firmware/libs/kaptan-mavlink/src/transmitter.rs
+51 -91
View File
@@ -59,20 +59,6 @@ where
Memory: DmaTxBufferFactory,
Engine: DmaTxEngine,
{
/// Admits an already-serialized MAVLink frame for UART transmission.
///
/// A full queue is a normal bounded-admission outcome, returned as
/// [`EnqueueResult`]. DMA preparation and transfer-start failures remain
/// [`MavlinkTransmitFault`] errors.
pub fn send_frame(
&mut self,
class: FrameClass,
frame: MavlinkFrame,
) -> Result<EnqueueResult, MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
let outcome = self.admit_and_start(class, frame);
outcome.start.map(|()| outcome.admission)
}
/// Admits one frame and attempts one bounded DMA start while preserving
/// both outcomes. A start failure retains the selected frame in the queue
/// reservation for a later service-turn retry; it does not change whether
@@ -82,34 +68,11 @@ where
class: FrameClass,
frame: MavlinkFrame,
) -> AdmissionStart<Memory::Error, Engine::StartError> {
let admission = self.enqueue_frame(class, frame);
let start = self.start_pending();
let admission = self.transmit.enqueue(PendingFrame { class, frame });
let start = self.start_next();
AdmissionStart { admission, start }
}
/// 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.
///
/// The returned class is the frame that reached a terminal DMA result,
@@ -122,19 +85,12 @@ where
) -> Result<Option<FrameClass>, MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
let completed = self.complete_dma(completion)?;
if completion == TxCompletion::Complete {
self.start_pending()?;
self.start_next()?;
}
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(
fn complete_dma(
&mut self,
completion: TxCompletion,
) -> Result<Option<FrameClass>, MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
@@ -164,8 +120,10 @@ where
#[cfg(test)]
mod tests {
use super::{EnqueueResult, FrameClass, MavlinkTransmitFault, MavlinkTransmitter};
use crate::{encoder::MavlinkEncoder, message::MavState};
use super::{
EnqueueResult, FrameClass, MavlinkTransmitFault, MavlinkTransmitter, PendingFrame,
};
use crate::encoder::MavlinkEncoder;
use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion};
struct Memory;
@@ -231,18 +189,18 @@ mod tests {
fn transfer_fault_drops_only_the_active_frame() {
let mut encoder = MavlinkEncoder::new(1, 1);
let mut transmitter = MavlinkTransmitter::<Memory, Engine, 2>::new(Memory, Engine);
transmitter
.send_frame(
FrameClass::BestEffort,
encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
)
.unwrap();
transmitter
.send_frame(
FrameClass::BestEffort,
encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
)
.unwrap();
assert!(
transmitter
.admit_and_start(FrameClass::BestEffort, encoder.heartbeat())
.start
.is_ok()
);
assert!(
transmitter
.admit_and_start(FrameClass::BestEffort, encoder.heartbeat())
.start
.is_ok()
);
assert!(transmitter.transmit.is_active());
transmitter
@@ -258,29 +216,20 @@ mod tests {
assert_eq!(
transmitter
.send_frame(
FrameClass::Critical,
encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
)
.unwrap(),
.admit_and_start(FrameClass::Critical, encoder.heartbeat())
.admission,
EnqueueResult::Enqueued
);
assert_eq!(
transmitter
.send_frame(
FrameClass::Critical,
encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
)
.unwrap(),
.admit_and_start(FrameClass::Critical, encoder.heartbeat())
.admission,
EnqueueResult::Enqueued
);
assert_eq!(
transmitter
.send_frame(
FrameClass::Critical,
encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
)
.unwrap(),
.admit_and_start(FrameClass::Critical, encoder.heartbeat())
.admission,
EnqueueResult::CriticalQueueFull
);
}
@@ -295,12 +244,16 @@ mod tests {
starts: 0,
},
);
let state = MavState::MAV_STATE_STANDBY;
transmitter.enqueue_frame(FrameClass::Critical, encoder.heartbeat(state, false));
transmitter.start_pending().unwrap();
transmitter.transmit.enqueue(PendingFrame {
class: FrameClass::Critical,
frame: encoder.heartbeat(),
});
transmitter.start_next().unwrap();
assert_eq!(transmitter.engine.starts, 1);
transmitter.enqueue_frame(FrameClass::Critical, encoder.heartbeat(state, false));
transmitter.transmit.enqueue(PendingFrame {
class: FrameClass::Critical,
frame: encoder.heartbeat(),
});
// Model the STM32 case: TCIF is acknowledged but CR.EN still reports
// busy for the first attempt to start the successor. `complete_dma`
@@ -312,12 +265,12 @@ mod tests {
assert_eq!(transmitter.engine.starts, 1);
transmitter.engine.busy_once = true;
assert!(matches!(
transmitter.start_pending(),
transmitter.start_next(),
Err(MavlinkTransmitFault::Engine(RetryError::Busy))
));
assert!(!transmitter.transmit.is_active());
transmitter.start_pending().unwrap();
transmitter.start_next().unwrap();
assert!(transmitter.transmit.is_active());
assert_eq!(transmitter.engine.starts, 2);
}
@@ -332,20 +285,27 @@ mod tests {
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));
transmitter.transmit.enqueue(PendingFrame {
class: FrameClass::Critical,
frame: encoder.heartbeat(),
});
transmitter.start_next().unwrap();
transmitter.transmit.enqueue(PendingFrame {
class: FrameClass::BestEffort,
frame: encoder.heartbeat(),
});
// 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));
transmitter.start_next().unwrap();
transmitter.transmit.enqueue(PendingFrame {
class: FrameClass::Critical,
frame: encoder.heartbeat(),
});
assert_eq!(transmitter.engine.starts, 2);
assert_eq!(&transmitter.engine.started_sequences[..2], &[0, 1]);