libs/kaptan-mavlink: add optical-flow encoding

libs/kaptan-mavlink/src/commands/mod.rs

@@ -0,0 +1,4 @@
+//! MAVLink command decoders.
+
+/// Targeted arm/disarm command decoding.
+pub mod arm;

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());
+    }
 }

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.