diff --git a/libs/kaptan-mavlink/src/arm.rs b/libs/kaptan-mavlink/src/commands/arm.rs similarity index 100% rename from libs/kaptan-mavlink/src/arm.rs rename to libs/kaptan-mavlink/src/commands/arm.rs diff --git a/libs/kaptan-mavlink/src/commands/mod.rs b/libs/kaptan-mavlink/src/commands/mod.rs new file mode 100644 index 0000000..54bdbbc --- /dev/null +++ b/libs/kaptan-mavlink/src/commands/mod.rs @@ -0,0 +1,4 @@ +//! MAVLink command decoders. + +/// Targeted arm/disarm command decoding. +pub mod arm; diff --git a/libs/kaptan-mavlink/src/encoder.rs b/libs/kaptan-mavlink/src/encoder.rs index 748eb3c..d4d558f 100644 --- a/libs/kaptan-mavlink/src/encoder.rs +++ b/libs/kaptan-mavlink/src/encoder.rs @@ -1,11 +1,25 @@ //! MAVLink frame encoding API. use kaptan_sensor::imu::ImuSnapshot; +use kaptan_sensor::optical_flow::OpticalFlowSnapshot; use kaptan_sensor::range::RangeSnapshot; use mavlink::MessageData; use crate::{MavlinkFrame, message}; +/// Board-selected MAVLink metadata for a range sensor. +#[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, +} + /// Serializes FC-domain data with MAVLink sender identity and sequence state. pub struct MavlinkEncoder { system_id: u8, @@ -90,35 +104,56 @@ impl MavlinkEncoder { pub fn distance_sensor_snapshot( &mut self, snapshot: &RangeSnapshot, - sensor_id: u8, - orientation: message::MavSensorOrientation, - minimum_distance_centimetres: u16, - maximum_distance_centimetres: u16, + config: DistanceSensorConfig, ) -> MavlinkFrame { let current_distance = (snapshot.distance_millimetres / 10).clamp( - u32::from(minimum_distance_centimetres), - u32::from(maximum_distance_centimetres), + 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: minimum_distance_centimetres, - max_distance: maximum_distance_centimetres, + min_distance: config.minimum_distance_centimetres, + max_distance: config.maximum_distance_centimetres, current_distance, mavtype: message::MavDistanceSensor::MAV_DISTANCE_SENSOR_LASER, - id: sensor_id, - orientation, + id: config.sensor_id, + orientation: config.orientation, covariance: u8::MAX, }; self.serialize(&distance_sensor) } + + /// Encodes a standard `OPTICAL_FLOW` frame from a ground-referenced flow + /// observation. + 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, + quality: snapshot.quality, + ground_distance: snapshot.ground_distance_millimetres as f32 / 1_000.0, + }; + + self.serialize(&optical_flow) + } } #[cfg(test)] mod tests { - use super::MavlinkEncoder; + use super::{DistanceSensorConfig, MavlinkEncoder}; use crate::message::MavState; use kaptan_sensor::imu::ImuSnapshot; + use kaptan_sensor::optical_flow::OpticalFlowSnapshot; use kaptan_sensor::range::RangeSnapshot; use kaptan_time::Timestamp; @@ -175,10 +210,12 @@ mod tests { let frame = encoder.distance_sensor_snapshot( &snapshot, - 0, - crate::message::MavSensorOrientation::MAV_SENSOR_ROTATION_PITCH_270, - 20, - 4_000, + 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(); @@ -187,4 +224,24 @@ mod tests { // current distance field follows time, minimum, and maximum range. assert_eq!(&bytes[18..20], &291u16.to_le_bytes()); } + + #[test] + fn optical_flow_snapshot_uses_the_optical_flow_message() { + let mut encoder = MavlinkEncoder::new(1, 1); + let snapshot = OpticalFlowSnapshot::new( + Timestamp::from_microseconds(123_000), + [1_000, -500], + 2_000, + 200, + ); + + let frame = encoder.optical_flow_snapshot(&snapshot, 3); + let bytes = frame.raw_bytes(); + + 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()); + } } diff --git a/libs/kaptan-mavlink/src/lib.rs b/libs/kaptan-mavlink/src/lib.rs index 3fefb94..892cf17 100644 --- a/libs/kaptan-mavlink/src/lib.rs +++ b/libs/kaptan-mavlink/src/lib.rs @@ -2,8 +2,8 @@ #![no_std] -/// Narrow MAVLink arm/disarm command decoding. -pub mod arm; +/// MAVLink command decoders. +pub mod commands; /// MAVLink frame encoding API for boards. pub mod encoder; /// Serial-byte MAVLink receive API for boards.