From f22b45c70aeaf8f367ff87da9a389e4b87b94841 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Sun, 26 Jul 2026 18:04:03 +0300 Subject: [PATCH] libs/kaptan-mavlink: add distance sensor telemetry --- libs/kaptan-mavlink/src/encoder.rs | 56 +++++++++++++++++++++++++- libs/kaptan-mavlink/src/transmitter.rs | 48 +++++++++++++++++++--- 2 files changed, 97 insertions(+), 7 deletions(-) diff --git a/libs/kaptan-mavlink/src/encoder.rs b/libs/kaptan-mavlink/src/encoder.rs index 2d06b49..748eb3c 100644 --- a/libs/kaptan-mavlink/src/encoder.rs +++ b/libs/kaptan-mavlink/src/encoder.rs @@ -1,6 +1,7 @@ //! MAVLink frame encoding API. -use kaptan_sensor::ImuSnapshot; +use kaptan_sensor::imu::ImuSnapshot; +use kaptan_sensor::range::RangeSnapshot; use mavlink::MessageData; use crate::{MavlinkFrame, message}; @@ -81,13 +82,44 @@ impl MavlinkEncoder { self.serialize(&raw_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. + pub fn distance_sensor_snapshot( + &mut self, + snapshot: &RangeSnapshot, + sensor_id: u8, + orientation: message::MavSensorOrientation, + minimum_distance_centimetres: u16, + maximum_distance_centimetres: u16, + ) -> MavlinkFrame { + let current_distance = (snapshot.distance_millimetres / 10).clamp( + u32::from(minimum_distance_centimetres), + u32::from(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: minimum_distance_centimetres, + max_distance: maximum_distance_centimetres, + current_distance, + mavtype: message::MavDistanceSensor::MAV_DISTANCE_SENSOR_LASER, + id: sensor_id, + orientation, + covariance: u8::MAX, + }; + + self.serialize(&distance_sensor) + } } #[cfg(test)] mod tests { use super::MavlinkEncoder; use crate::message::MavState; - use kaptan_sensor::ImuSnapshot; + use kaptan_sensor::imu::ImuSnapshot; + use kaptan_sensor::range::RangeSnapshot; use kaptan_time::Timestamp; #[test] @@ -135,4 +167,24 @@ mod tests { assert_ne!(frame.raw_bytes()[16] & 0x80, 0); } + + #[test] + fn range_snapshot_uses_distance_sensor_centimetres() { + 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, + 0, + crate::message::MavSensorOrientation::MAV_SENSOR_ROTATION_PITCH_270, + 20, + 4_000, + ); + let bytes = frame.raw_bytes(); + + 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()); + } } diff --git a/libs/kaptan-mavlink/src/transmitter.rs b/libs/kaptan-mavlink/src/transmitter.rs index 8bd47c4..d27f6b9 100644 --- a/libs/kaptan-mavlink/src/transmitter.rs +++ b/libs/kaptan-mavlink/src/transmitter.rs @@ -6,7 +6,7 @@ use kaptan_tx::{PendingFrame, TransmitQueue}; use crate::MavlinkFrame; /// Delivery importance used for bounded MAVLink transmission admission. -pub use kaptan_tx::FrameClass; +pub use kaptan_tx::{EnqueueResult, FrameClass}; /// Failure while preparing or starting a MAVLink frame transfer. #[derive(Debug)] @@ -45,13 +45,17 @@ where 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<(), MavlinkTransmitFault> { - let _ = self.transmit.enqueue(PendingFrame { class, frame }); - self.start_next() + ) -> Result> { + let admission = self.transmit.enqueue(PendingFrame { class, frame }); + self.start_next().map(|()| admission) } /// Releases a terminal DMA frame and opportunistically starts the next. @@ -85,7 +89,7 @@ where #[cfg(test)] mod tests { - use super::{FrameClass, MavlinkTransmitter}; + use super::{EnqueueResult, FrameClass, MavlinkTransmitter}; use crate::{encoder::MavlinkEncoder, message::MavState}; use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion}; @@ -131,4 +135,38 @@ mod tests { transmitter.on_dma_complete(TxCompletion::TransferFault); assert!(!transmitter.transmit.is_active()); } + + #[test] + fn reports_critical_queue_saturation_to_the_policy_owner() { + let mut encoder = MavlinkEncoder::new(1, 1); + let mut transmitter = MavlinkTransmitter::::new(Memory, Engine); + + assert_eq!( + transmitter + .send_frame( + FrameClass::Critical, + encoder.heartbeat(MavState::MAV_STATE_STANDBY, false), + ) + .unwrap(), + EnqueueResult::Enqueued + ); + assert_eq!( + transmitter + .send_frame( + FrameClass::Critical, + encoder.heartbeat(MavState::MAV_STATE_STANDBY, false), + ) + .unwrap(), + EnqueueResult::Enqueued + ); + assert_eq!( + transmitter + .send_frame( + FrameClass::Critical, + encoder.heartbeat(MavState::MAV_STATE_STANDBY, false), + ) + .unwrap(), + EnqueueResult::CriticalQueueFull + ); + } }