262 lines
9.2 KiB
Rust
262 lines
9.2 KiB
Rust
//! 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<M: MessageData>(&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<M: MessageData>(&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());
|
|
}
|
|
}
|