diff --git a/libs/kaptan-mavlink/src/lib.rs b/libs/kaptan-mavlink/src/lib.rs index d8fc345..c70d876 100644 --- a/libs/kaptan-mavlink/src/lib.rs +++ b/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(&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]); + } +}