//! MAVLink frame encoding API. #[cfg(feature = "sensor-telemetry")] use kaptan_sensor::imu::ImuSnapshot; #[cfg(feature = "sensor-telemetry")] use kaptan_sensor::optical_flow::OpticalFlowSnapshot; #[cfg(feature = "sensor-telemetry")] use kaptan_sensor::range::RangeSnapshot; use mavlink::MessageData; use crate::{MavlinkFrame, message}; /// Board-selected MAVLink metadata for a range sensor. #[cfg(feature = "sensor-telemetry")] #[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, component_id: u8, next_sequence: u8, } impl MavlinkEncoder { /// Creates an encoder 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, } } fn serialize(&mut self, message: &M) -> MavlinkFrame { let mut raw = MavlinkFrame::new(); raw.serialize_message_data( crate::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 } /// Encodes one generated MAVLink data record through this endpoint's sole /// header-sequence owner. Runtime-log/config adapters return data records /// and must use this method rather than create a competing encoder. pub fn custom(&mut self, message: &M) -> MavlinkFrame { self.serialize(message) } /// Encodes a board-I/O heartbeat with its current arming state. pub fn heartbeat(&mut self, system_status: message::MavState, armed: bool) -> MavlinkFrame { let heartbeat = message::HEARTBEAT_DATA { custom_mode: 0, mavtype: message::MavType::MAV_TYPE_ONBOARD_CONTROLLER, autopilot: message::MavAutopilot::MAV_AUTOPILOT_INVALID, base_mode: if armed { message::MavModeFlag::MAV_MODE_FLAG_SAFETY_ARMED } else { message::MavModeFlag::empty() }, system_status, mavlink_version: 3, }; self.serialize(&heartbeat) } /// Encodes a standard `RAW_IMU` frame. #[cfg(feature = "sensor-telemetry")] 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) } /// 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. #[cfg(feature = "sensor-telemetry")] pub fn distance_sensor_snapshot( &mut self, snapshot: &RangeSnapshot, config: DistanceSensorConfig, ) -> MavlinkFrame { let current_distance = (snapshot.distance_millimetres / 10).clamp( 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: config.minimum_distance_centimetres, max_distance: config.maximum_distance_centimetres, current_distance, mavtype: message::MavDistanceSensor::MAV_DISTANCE_SENSOR_LASER, 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. #[cfg(feature = "sensor-telemetry")] 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 { #[cfg(feature = "sensor-telemetry")] use super::DistanceSensorConfig; use super::MavlinkEncoder; use crate::message::MavState; #[cfg(feature = "sensor-telemetry")] use kaptan_sensor::imu::ImuSnapshot; #[cfg(feature = "sensor-telemetry")] use kaptan_sensor::optical_flow::OpticalFlowSnapshot; #[cfg(feature = "sensor-telemetry")] use kaptan_sensor::range::RangeSnapshot; #[cfg(feature = "sensor-telemetry")] use kaptan_time::Timestamp; #[test] fn heartbeat_has_the_configured_sender_and_sequence() { let mut encoder = MavlinkEncoder::new(42, 7); let frame = encoder.heartbeat(MavState::MAV_STATE_STANDBY, false); 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 encoder = MavlinkEncoder::new(1, 1); let frame = encoder.heartbeat(MavState::MAV_STATE_CRITICAL, false); assert_eq!(frame.raw_bytes()[17], MavState::MAV_STATE_CRITICAL as u8); } #[cfg(feature = "sensor-telemetry")] #[test] fn snapshot_adapter_uses_flight_data_timestamp_and_units() { let mut encoder = MavlinkEncoder::new(1, 1); let snapshot = ImuSnapshot::new( Timestamp::from_microseconds(123), [1, 2, 3], [4, 5, 6], 2_500, ); let frame = encoder.raw_imu_snapshot(&snapshot); assert_eq!(&frame.raw_bytes()[7..10], &[27, 0, 0]); } #[test] fn armed_heartbeat_sets_the_standard_armed_flag() { let mut encoder = MavlinkEncoder::new(1, 1); let frame = encoder.heartbeat(MavState::MAV_STATE_ACTIVE, true); assert_ne!(frame.raw_bytes()[16] & 0x80, 0); } #[cfg(feature = "sensor-telemetry")] #[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, 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(); 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()); } #[cfg(feature = "sensor-telemetry")] #[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()); } }