support bi-directional mavlink
Signed-off-by: Onur Özkan <work@onurozkan.dev>
This commit is contained in:
Onur Özkan committed
1 parent
2d35d935b4
commit
d32d9f4d36
16 files changed
+651
-344
No files matched your search
@@ -1,8 +1,9 @@
|
||||
//! RTIC- and PAC-independent flight services.
|
||||
//!
|
||||
//! Acquisition and telemetry have separate owners. Boards connect hardware
|
||||
//! interrupts to their dedicated workers through bounded deferred events; they
|
||||
//! must not place both services behind one priority-ceiling resource.
|
||||
//! Acquisition services and board-owned output workers have separate owners.
|
||||
//! Boards connect hardware interrupts to their dedicated workers through
|
||||
//! bounded deferred events; they must not place unrelated work behind one
|
||||
//! priority-ceiling resource.
|
||||
|
||||
#![no_std]
|
||||
|
||||
@@ -10,9 +11,6 @@
|
||||
pub mod imu;
|
||||
/// Lock-free worker handoff primitives.
|
||||
pub mod mailbox;
|
||||
/// Single-owner MAVLink and UART transmit service.
|
||||
pub mod telemetry;
|
||||
|
||||
pub use imu::{ImuEvent, ImuService};
|
||||
pub use mailbox::LatestImuMailbox;
|
||||
pub use telemetry::{TelemetryFault, TelemetryService};
|
||||
@@ -7,11 +7,11 @@ use kaptan_time::Timestamp;
|
||||
|
||||
/// A latest-value IMU mailbox.
|
||||
///
|
||||
/// One IMU worker publishes and one telemetry worker consumes. Publishing a
|
||||
/// newer sample intentionally replaces an unread older sample: telemetry is a
|
||||
/// best-effort observer and must never delay acquisition. The sequence counter
|
||||
/// is a seqlock; odd values indicate a writer and even values identify a
|
||||
/// stable snapshot.
|
||||
/// One IMU worker publishes and one output worker consumes. Publishing a newer
|
||||
/// sample intentionally replaces an unread older sample: an output observer is
|
||||
/// best-effort and must never delay acquisition. The sequence counter is a
|
||||
/// seqlock; odd values indicate a writer and even values identify a stable
|
||||
/// snapshot.
|
||||
pub struct LatestImuMailbox {
|
||||
sequence: AtomicU32,
|
||||
timestamp_low: AtomicU32,
|
||||
|
||||
@@ -1,143 +0,0 @@
|
||||
//! Single-owner MAVLink transmit service.
|
||||
|
||||
use kaptan_dma::{DmaTxBufferFactory, DmaTxEngine, TxCompletion};
|
||||
use kaptan_mavlink::{MavlinkFrame, MavlinkSender, message::MavState};
|
||||
use kaptan_sensor::ImuSnapshot;
|
||||
use kaptan_tx::{FrameClass, PendingFrame, TransmitQueue};
|
||||
|
||||
/// Failure while preparing or starting one telemetry frame.
|
||||
#[derive(Debug)]
|
||||
pub enum TelemetryFault<MemoryError, EngineError> {
|
||||
/// DMA-visible frame preparation failed.
|
||||
Memory(MemoryError),
|
||||
/// The UART DMA route rejected a transfer start.
|
||||
Engine(EngineError),
|
||||
}
|
||||
|
||||
/// Owns MAVLink packet sequencing and UART DMA frame lifecycle.
|
||||
///
|
||||
/// Exactly one worker invokes this service. It is not shared with sensor or
|
||||
/// hardware IRQ execution.
|
||||
pub struct TelemetryService<Memory, Engine, const N: usize> {
|
||||
mavlink: MavlinkSender,
|
||||
transmit: TransmitQueue<MavlinkFrame, N>,
|
||||
memory: Memory,
|
||||
engine: Engine,
|
||||
}
|
||||
|
||||
impl<Memory, Engine, const N: usize> TelemetryService<Memory, Engine, N> {
|
||||
/// Creates an empty telemetry service.
|
||||
pub const fn new(system_id: u8, component_id: u8, memory: Memory, engine: Engine) -> Self {
|
||||
Self {
|
||||
mavlink: MavlinkSender::new(system_id, component_id),
|
||||
transmit: TransmitQueue::new(),
|
||||
memory,
|
||||
engine,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<Memory, Engine, const N: usize> TelemetryService<Memory, Engine, N>
|
||||
where
|
||||
Memory: DmaTxBufferFactory,
|
||||
Engine: DmaTxEngine,
|
||||
{
|
||||
/// Queues the newest accepted IMU observation.
|
||||
pub fn publish_sample(
|
||||
&mut self,
|
||||
sample: &ImuSnapshot,
|
||||
) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
|
||||
let frame = self.mavlink.raw_imu_snapshot(sample);
|
||||
self.enqueue_and_start(FrameClass::BestEffort, frame)
|
||||
}
|
||||
|
||||
/// Queues one liveness heartbeat.
|
||||
pub fn publish_heartbeat(
|
||||
&mut self,
|
||||
) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
|
||||
let frame = self.mavlink.heartbeat(MavState::MAV_STATE_STANDBY);
|
||||
self.enqueue_and_start(FrameClass::Critical, frame)
|
||||
}
|
||||
|
||||
/// Releases a terminal DMA frame and opportunistically starts the next.
|
||||
///
|
||||
/// Telemetry is best-effort: a terminal route fault drops only the active
|
||||
/// frame and a rejected follow-up start is retried on later publication.
|
||||
pub fn on_dma_complete(&mut self, completion: TxCompletion) {
|
||||
match completion {
|
||||
TxCompletion::NotPending => {}
|
||||
TxCompletion::Complete => {
|
||||
let _ = self.transmit.complete();
|
||||
let _ = self.start_next();
|
||||
}
|
||||
TxCompletion::TransferFault | TxCompletion::Halted => {
|
||||
let _ = self.transmit.fail();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn enqueue_and_start(
|
||||
&mut self,
|
||||
class: FrameClass,
|
||||
frame: MavlinkFrame,
|
||||
) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
|
||||
let _ = self.transmit.enqueue(PendingFrame { class, frame });
|
||||
self.start_next()
|
||||
}
|
||||
|
||||
fn start_next(&mut self) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
|
||||
let (transmit, memory, engine) = (&mut self.transmit, &mut self.memory, &mut self.engine);
|
||||
transmit
|
||||
.start_with(|frame| {
|
||||
let buffer = memory
|
||||
.prepare(frame.raw_bytes())
|
||||
.map_err(TelemetryFault::Memory)?;
|
||||
engine.start(buffer).map_err(TelemetryFault::Engine)
|
||||
})
|
||||
.map(|_| ())
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::TelemetryService;
|
||||
use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion};
|
||||
use kaptan_sensor::ImuSnapshot;
|
||||
use kaptan_time::Timestamp;
|
||||
|
||||
struct Memory;
|
||||
|
||||
impl DmaTxBufferFactory for Memory {
|
||||
type Error = core::convert::Infallible;
|
||||
|
||||
fn prepare<'a>(&mut self, bytes: &'a [u8]) -> Result<DmaReadableBuffer<'a>, Self::Error> {
|
||||
// SAFETY: this host-only test keeps the queue's active frame alive.
|
||||
Ok(unsafe { DmaReadableBuffer::new_unchecked(bytes) })
|
||||
}
|
||||
}
|
||||
|
||||
struct Engine;
|
||||
|
||||
impl DmaTxEngine for Engine {
|
||||
type StartError = core::convert::Infallible;
|
||||
|
||||
fn start(&mut self, _: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> {
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
fn sample(timestamp: u64) -> ImuSnapshot {
|
||||
ImuSnapshot::new(Timestamp::from_microseconds(timestamp), [0; 3], [0; 3], 0)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn transfer_fault_drops_only_the_active_frame() {
|
||||
let mut service = TelemetryService::<Memory, Engine, 2>::new(1, 1, Memory, Engine);
|
||||
service.publish_sample(&sample(1)).unwrap();
|
||||
service.publish_sample(&sample(2)).unwrap();
|
||||
assert!(service.transmit.is_active());
|
||||
|
||||
service.on_dma_complete(TxCompletion::TransferFault);
|
||||
assert!(!service.transmit.is_active());
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user