libs/kaptan-mavlink: add heartbeat and RAW_IMU sender

libs/kaptan-mavlink/src/lib.rs

@@ -1,7 +1,79 @@
-//! MAVLink support for Kaptan.
+//! MAVLink protocol adapter for Kaptan.
 
 #![no_std]
 
+use kaptan_sensor::ImuSnapshot;
+use mavlink::MessageData;
+
+/// An owned MAVLink 2 frame supplied by rust-mavlink.
+pub type MavlinkFrame = message::MAVLinkV2MessageRaw;
+
+/// Owns MAVLink sender identity and packet sequence state.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct MavlinkSender {
+    system_id: u8,
+    component_id: u8,
+    next_sequence: u8,
+}
+
+impl MavlinkSender {
+    /// Creates a sender beginning with MAVLink packet sequence zero.
+    pub const fn new(system_id: u8, component_id: u8) -> Self {
+        Self {
+            system_id,
+            component_id,
+            next_sequence: 0,
+        }
+    }
+
+    /// Serializes a Kaptan heartbeat with the supplied system-health state.
+    pub fn heartbeat(&mut self, system_status: message::MavState) -> MavlinkFrame {
+        let heartbeat = message::HEARTBEAT_DATA {
+            custom_mode: 0,
+            mavtype: message::MavType::MAV_TYPE_QUADROTOR,
+            autopilot: message::MavAutopilot::MAV_AUTOPILOT_GENERIC,
+            base_mode: message::MavModeFlag::empty(),
+            system_status,
+            mavlink_version: 3,
+        };
+
+        self.serialize(&heartbeat)
+    }
+
+    /// Serializes a standard `RAW_IMU` frame from a flight-data snapshot.
+    pub fn raw_imu_snapshot(&mut self, snapshot: &ImuSnapshot) -> MavlinkFrame {
+        let raw_imu = message::RAW_IMU_DATA {
+            time_usec: snapshot.timestamp.as_microseconds(),
+            xacc: snapshot.acceleration_milligravity[0],
+            yacc: snapshot.acceleration_milligravity[1],
+            zacc: snapshot.acceleration_milligravity[2],
+            xgyro: snapshot.angular_rate_milliradians_per_second[0],
+            ygyro: snapshot.angular_rate_milliradians_per_second[1],
+            zgyro: snapshot.angular_rate_milliradians_per_second[2],
+            xmag: 0,
+            ymag: 0,
+            zmag: 0,
+        };
+
+        self.serialize(&raw_imu)
+    }
+
+    fn serialize<M: MessageData>(&mut self, message: &M) -> MavlinkFrame {
+        let mut raw = message::MAVLinkV2MessageRaw::new();
+        raw.serialize_message_data(
+            message::MavHeader {
+                sequence: self.next_sequence,
+                system_id: self.system_id,
+                component_id: self.component_id,
+            },
+            message,
+        );
+        self.next_sequence = self.next_sequence.wrapping_add(1);
+
+        raw
+    }
+}
+
 /// Supported MAVLink message types for Kaptan.
 pub mod message {
     pub use mavlink::MAVLinkV2MessageRaw;
@@ -11,3 +83,53 @@ pub mod message {
     #[cfg(kaptan_has_messages)]
     include!(concat!(env!("OUT_DIR"), "/mod.rs"));
 }
+
+#[cfg(test)]
+mod tests {
+    use super::MavlinkSender;
+    use kaptan_sensor::ImuSnapshot;
+    use kaptan_time::Timestamp;
+
+    #[test]
+    fn heartbeat_has_the_configured_sender_and_sequence() {
+        let mut sender = MavlinkSender::new(42, 7);
+        let frame = sender.heartbeat(crate::message::MavState::MAV_STATE_STANDBY);
+        let bytes = frame.raw_bytes();
+
+        assert_eq!(bytes[0], 0xFD);
+        assert_eq!(bytes[2], 0);
+        assert_eq!(bytes[3], 0);
+        assert_eq!(bytes[4], 0);
+        assert_eq!(bytes[5], 42);
+        assert_eq!(bytes[6], 7);
+        assert_eq!(&bytes[7..10], &[0, 0, 0]);
+    }
+
+    #[test]
+    fn heartbeat_preserves_the_requested_health_state() {
+        let mut sender = MavlinkSender::new(1, 1);
+        let frame = sender.heartbeat(crate::message::MavState::MAV_STATE_CRITICAL);
+
+        // MAVLink 2 header is 10 octets; HEARTBEAT.system_status is payload
+        // octet seven according to the common dialect wire layout.
+        assert_eq!(
+            frame.raw_bytes()[17],
+            crate::message::MavState::MAV_STATE_CRITICAL as u8
+        );
+    }
+
+    #[test]
+    fn snapshot_adapter_uses_flight_data_timestamp_and_units() {
+        let mut sender = MavlinkSender::new(1, 1);
+        let snapshot = ImuSnapshot::new(
+            Timestamp::from_microseconds(123),
+            [1, 2, 3],
+            [4, 5, 6],
+            2_500,
+        );
+
+        let frame = sender.raw_imu_snapshot(&snapshot);
+
+        assert_eq!(&frame.raw_bytes()[7..10], &[27, 0, 0]);
+    }
+}