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

feat(f722): add usb mavlink transport

Browse Source
This commit is contained in:
Onur Özkan committed 2026-09-06 23:14:00 +03:00
1 parent 7dc3ccb619
commit 604a7aba86
18 files changed
+1087 -208
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

firmware/libs/kaptan-data-channel/Cargo.toml
+1 -1
View File
@@ -10,10 +10,10 @@ workspace = true
[dependencies]
kaptan-err-reportable = { path = "../kaptan-err-reportable" }
kaptan-dma = { path = "../kaptan-dma" }
kaptan-mavlink = { path = "../kaptan-mavlink", default-features = false, features = [
"sensor-telemetry",
] }
kaptan-sensor = { path = "../kaptan-sensor" }
kaptan-time = { path = "../kaptan-time" }
kaptan-tx = { path = "../kaptan-tx" }
mavlink-core = { version = "0.18", default-features = false }
firmware/libs/kaptan-data-channel/src/lib.rs
+57 -108
View File
@@ -1,21 +1,15 @@
//! Complete outbound MAVLink data flow.
//! Outbound MAVLink frame production.
//!
//! This crate owns fixed-rate scheduling, mailbox consumption, vertical-data
//! freshness, MAVLink encoding, and bounded queue classification. A platform
//! supplies only mailbox references, vertical-data freshness, and a transport
//! sink.
//! freshness, MAVLink encoding, and frame classification. Platforms retain
//! transport ownership and decide where each produced frame is admitted.
#![no_std]
use kaptan_dma::TxCompletion;
use kaptan_err_reportable::KaptanError;
use kaptan_mavlink::{
MavlinkFrame,
encoder::MavlinkEncoder,
generated::kaptan::MavMessage,
transmitter::{EnqueueResult, FrameClass},
};
use kaptan_mavlink::{MavlinkFrame, encoder::MavlinkEncoder, generated::kaptan::MavMessage};
use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
use kaptan_tx::{FrameClass, PendingFrame};
use mavlink_core::MessageData;
mod mailbox;
@@ -39,19 +33,6 @@ pub struct DataChannelRates {
pub heartbeat_hz: usize,
}
/// Platform transport used by a data channel.
///
/// The channel decides exactly which encoded frame is admitted and with which
/// delivery class. The platform retains only DMA endpoint ownership.
pub trait FrameSink {
/// Admits one fully encoded frame to the platform's bounded transport.
fn send(&mut self, class: FrameClass, frame: MavlinkFrame) -> EnqueueResult;
/// Applies one terminal DMA interrupt and starts the next retained frame
/// when possible.
fn on_dma_complete(&mut self, completion: TxCompletion);
}
/// Mailboxes read by one data-channel worker.
#[derive(Clone, Copy)]
pub struct DataSources<'a> {
@@ -77,9 +58,8 @@ pub enum DataChannelError {
ScheduleFull,
}
/// Complete outbound data flow for one MAVLink endpoint.
pub struct DataChannel<'a, Sink, const SERVICE_HZ: usize> {
sink: Sink,
/// Scheduled outbound MAVLink frame producer.
pub struct DataChannel<'a, const SERVICE_HZ: usize> {
encoder: MavlinkEncoder,
schedule: Schedule<SERVICE_HZ>,
sources: DataSources<'a>,
@@ -93,13 +73,9 @@ pub struct DataChannel<'a, Sink, const SERVICE_HZ: usize> {
optical_flow_age_ticks: u8,
}
impl<'a, Sink, const SERVICE_HZ: usize> DataChannel<'a, Sink, SERVICE_HZ>
where
Sink: FrameSink,
{
impl<'a, const SERVICE_HZ: usize> DataChannel<'a, SERVICE_HZ> {
/// Creates an outbound data flow from already-validated runtime inputs.
pub fn new(
sink: Sink,
system_id: u8,
component_id: u8,
rates: DataChannelRates,
@@ -108,7 +84,6 @@ where
) -> Result<Self, DataChannelError> {
let schedule = Schedule::from_rates(rates)?;
Ok(Self {
sink,
encoder: MavlinkEncoder::new(system_id, component_id),
schedule,
sources,
@@ -125,57 +100,53 @@ where
/// Runs all outbound work due for one platform service tick.
///
/// No schedule token escapes this method. It refreshes cached vertical
/// observations, selects due data, encodes the MAVLink message, and admits
/// it with the correct bounded-queue class.
pub fn on_service_tick(&mut self) {
/// At most one classified frame is returned for transport admission.
pub fn on_service_tick(&mut self) -> Option<PendingFrame<MavlinkFrame>> {
self.refresh_vertical_data();
match self.schedule.next_due() {
Some(DueData::Imu) => {
if let Some(sample) = self.sources.imu.take_newer(&mut self.observed_imu) {
let _ = self
.sink
.send(FrameClass::BestEffort, self.encoder.imu_snapshot(&sample));
}
self.sources
.imu
.take_newer(&mut self.observed_imu)
.map(|sample| PendingFrame {
class: FrameClass::BestEffort,
frame: self.encoder.imu_snapshot(&sample),
})
}
Some(DueData::Distance) if self.range_age_ticks <= self.maximum_vertical_age_ticks => {
if let Some(range) = self.last_range {
let _ = self
.sink
.send(FrameClass::BestEffort, self.encoder.range_snapshot(&range));
}
self.last_range.map(|range| PendingFrame {
class: FrameClass::BestEffort,
frame: self.encoder.range_snapshot(&range),
})
}
Some(DueData::OpticalFlow)
if self.optical_flow_age_ticks <= self.maximum_vertical_age_ticks =>
{
if let Some(flow) = self.last_optical_flow {
let _ = self.sink.send(
FrameClass::BestEffort,
self.encoder.optical_flow_snapshot(&flow),
);
}
self.last_optical_flow.map(|flow| PendingFrame {
class: FrameClass::BestEffort,
frame: self.encoder.optical_flow_snapshot(&flow),
})
}
Some(DueData::Heartbeat) => {
let _ = self.sink.send(FrameClass::Vital, self.encoder.heartbeat());
}
Some(DueData::Distance | DueData::OpticalFlow) | None => {}
Some(DueData::Heartbeat) => Some(PendingFrame {
class: FrameClass::Vital,
frame: self.encoder.heartbeat(),
}),
Some(DueData::Distance | DueData::OpticalFlow) | None => None,
}
}
/// Completes the active DMA lifecycle for this channel's transport.
pub fn on_dma_complete(&mut self, completion: TxCompletion) {
self.sink.on_dma_complete(completion)
}
/// Encodes an adapter-owned generated MAVLink data record through the
/// channel's sole encoder and bounded transport sink.
pub fn send_message<M: MessageData<Message = MavMessage>>(
/// Encodes an adapter-owned generated MAVLink record through the channel's
/// sole sequence-number owner.
pub fn encode_message<M: MessageData<Message = MavMessage>>(
&mut self,
class: FrameClass,
message: &M,
) -> EnqueueResult {
self.sink.send(class, self.encoder.encode(message))
) -> PendingFrame<MavlinkFrame> {
PendingFrame {
class,
frame: self.encoder.encode(message),
}
}
fn refresh_vertical_data(&mut self) {
@@ -276,39 +247,15 @@ impl<const SERVICE_HZ: usize> Schedule<SERVICE_HZ> {
#[cfg(test)]
mod tests {
use super::{
DataChannel, DataChannelRates, DataSources, FrameSink, LatestImuMailbox,
LatestOpticalFlowMailbox, LatestRangeMailbox,
};
use kaptan_dma::TxCompletion;
use kaptan_mavlink::{
MavlinkFrame,
transmitter::{EnqueueResult, FrameClass},
DataChannel, DataChannelRates, DataSources, LatestImuMailbox, LatestOpticalFlowMailbox,
LatestRangeMailbox,
};
use kaptan_mavlink::generated::kaptan::MavMessage;
use kaptan_sensor::{
imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot,
};
use kaptan_time::Timestamp;
struct Sink {
best_effort: u8,
critical: u8,
completions: u8,
}
impl FrameSink for Sink {
fn send(&mut self, class: FrameClass, _: MavlinkFrame) -> EnqueueResult {
match class {
FrameClass::Vital => self.critical += 1,
FrameClass::BestEffort => self.best_effort += 1,
FrameClass::Critical => self.critical += 1,
}
EnqueueResult::Enqueued
}
fn on_dma_complete(&mut self, _: TxCompletion) {
self.completions += 1;
}
}
use kaptan_tx::FrameClass;
const RATES: DataChannelRates = DataChannelRates {
imu_hz: 20,
@@ -318,16 +265,11 @@ mod tests {
};
#[test]
fn owns_rate_selection_encoding_admission_and_completion() {
fn owns_rate_selection_encoding_and_classification() {
let imu = LatestImuMailbox::new();
let range = LatestRangeMailbox::new();
let flow = LatestOpticalFlowMailbox::new();
let mut channel: DataChannel<'_, Sink, 100> = DataChannel::new(
Sink {
best_effort: 0,
critical: 0,
completions: 0,
},
let mut channel: DataChannel<'_, 100> = DataChannel::new(
1,
1,
RATES,
@@ -339,6 +281,8 @@ mod tests {
},
)
.unwrap();
let mut best_effort = 0;
let mut vital = 0;
for tick in 0..100 {
imu.publish(ImuSnapshot::new(
@@ -354,12 +298,17 @@ mod tests {
100,
100,
));
channel.on_service_tick();
if let Some(frame) = channel.on_service_tick() {
assert!(frame.frame.has_valid_crc::<MavMessage>());
assert_eq!(frame.frame.sequence(), best_effort + vital);
match frame.class {
FrameClass::Vital => vital += 1,
FrameClass::BestEffort => best_effort += 1,
FrameClass::Critical => unreachable!(),
}
}
}
channel.on_dma_complete(TxCompletion::Complete);
let sink = channel.sink;
assert_eq!(sink.best_effort, 60);
assert_eq!(sink.critical, 2);
assert_eq!(sink.completions, 1);
assert_eq!(best_effort, 60);
assert_eq!(vital, 2);
}
}
firmware/libs/kaptan-mavlink/src/receiver.rs
+22 -8
View File
@@ -5,7 +5,7 @@ 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.
/// decode it once at protocol dispatch.
#[repr(transparent)]
pub struct IntegrityCheckedFrame(MavlinkFrame);
@@ -32,14 +32,20 @@ impl IntegrityCheckedFrame {
}
}
struct IntegrityFramer {
/// Incremental, unbuffered MAVLink 2 framing with Kaptan ID and CRC validation.
///
/// Consumers that can pause byte admission can process each returned frame
/// directly. Interrupt-driven byte streams can use [`IntegrityMavlinkReceiver`]
/// to queue complete frames instead. Accepted frames remain unauthenticated.
pub struct IntegrityMavlinkFramer {
frame: MavlinkFrame,
received: usize,
expected: usize,
}
impl IntegrityFramer {
const fn new() -> Self {
impl IntegrityMavlinkFramer {
/// Creates an empty framer.
pub const fn new() -> Self {
Self {
frame: MavlinkFrame::new(),
received: 0,
@@ -47,7 +53,8 @@ impl IntegrityFramer {
}
}
fn receive_byte(&mut self, byte: u8) -> Option<IntegrityCheckedFrame> {
/// Consumes one byte, returning a complete integrity-checked frame if ready.
pub fn receive_byte(&mut self, byte: u8) -> Option<IntegrityCheckedFrame> {
if self.received == 0 {
if byte == 0xfd {
self.frame.as_mut_slice()[0] = byte;
@@ -81,20 +88,27 @@ impl IntegrityFramer {
IntegrityCheckedFrame::try_new(frame)
}
fn reset(&mut self) {
/// Discards a partial frame after a stream discontinuity.
pub fn reset(&mut self) {
self.frame = MavlinkFrame::new();
self.received = 0;
self.expected = 0;
}
}
impl Default for IntegrityMavlinkFramer {
fn default() -> Self {
Self::new()
}
}
/// Bounded integrity-only MAVLink 2 receiver.
///
/// It rejects signed, incompatible, corrupt, and MAVLink 1 frames, but an
/// accepted frame is still unauthenticated. The composition decides whether
/// and how a received request may be admitted.
pub struct IntegrityMavlinkReceiver<const N: usize> {
framer: IntegrityFramer,
framer: IntegrityMavlinkFramer,
frames: [Option<IntegrityCheckedFrame>; N],
front: usize,
length: usize,
@@ -105,7 +119,7 @@ impl<const N: usize> IntegrityMavlinkReceiver<N> {
pub const fn new() -> Self {
assert!(N != 0, "integrity receiver capacity must be nonzero");
Self {
framer: IntegrityFramer::new(),
framer: IntegrityMavlinkFramer::new(),
frames: [const { None }; N],
front: 0,
length: 0,
firmware/libs/kaptan-tx/src/lib.rs
+14 -4
View File
@@ -1,7 +1,7 @@
//! Bounded protocol-neutral transmit admission and ownership lifecycle.
//!
//! Protocol encoders own frame bytes. This crate owns only bounded priority
//! admission and the transition from pending frame to active DMA ownership.
//! admission and the transition from pending frame to active transport ownership.
#![no_std]
@@ -9,7 +9,7 @@
#[derive(Clone, Copy, Debug, Eq, Ord, PartialEq, PartialOrd)]
pub enum FrameClass {
/// Heartbeat and current safety/health status. A pending vital frame may
/// displace any pending non-vital frame; active DMA storage is untouched.
/// displace any pending non-vital frame; active storage is untouched.
Vital,
/// Fault transitions and authoritative replies that best-effort samples
/// cannot displace.
@@ -147,12 +147,12 @@ impl<Frame: Copy, const N: usize> TransmitQueue<Frame, N> {
Ok(StartOutcome::Started)
}
/// Releases the active frame after a terminal DMA completion.
/// Releases the active frame after the transport no longer borrows it.
pub fn complete(&mut self) -> Option<PendingFrame<Frame>> {
self.active.take()
}
/// Releases the active frame after a terminal DMA fault.
/// Releases the active frame after a terminal transport fault.
pub fn fail(&mut self) -> Option<PendingFrame<Frame>> {
self.active.take()
}
@@ -162,6 +162,14 @@ impl<Frame: Copy, const N: usize> TransmitQueue<Frame, N> {
self.active.is_some()
}
/// Borrows the frame currently owned by the active transport operation.
///
/// Packetized transports use this to retain one selected frame while
/// advancing it across multiple bounded endpoint writes.
pub const fn active(&self) -> Option<&PendingFrame<Frame>> {
self.active.as_ref()
}
fn take_next(&mut self) -> Option<PendingFrame<Frame>> {
let vital = self
.pending
@@ -203,6 +211,7 @@ mod tests {
Ok(StartOutcome::Started)
);
assert!(queue.is_active());
assert_eq!(queue.active().map(|pending| pending.frame), Some(1));
assert_eq!(
queue.enqueue(PendingFrame {
@@ -212,6 +221,7 @@ mod tests {
EnqueueResult::Enqueued
);
assert_eq!(queue.complete().unwrap().frame, 1);
assert!(queue.active().is_none());
}
#[test]