mirror of
https://github.com/azaion/autopilot.git
synced 2026-06-21 10:21:10 +00:00
Oleksandr Bezdieniezhnykh
740bf37d76
Lands the second batch under epic AZ-626's implementation plan.
mavlink_layer (AZ-641 + AZ-642):
- Hand-rolled MAVLink v2 codec covering the §7.7 surface: HEARTBEAT,
SYS_STATUS, SET_MODE, ATTITUDE, GLOBAL_POSITION_INT, MISSION_* (7),
COMMAND_LONG, COMMAND_ACK, EXTENDED_SYS_STATE, STATUSTEXT (17 total).
- Streaming decoder demuxes arbitrary-sized byte arrivals, drops malformed
frames with typed parse-error counters (crc/truncated/unknown_id/seq_gap),
and surfaces sequence gaps without hard-failing the link.
- Encoder tracks the per-link tx_seq counter and applies the MAVLink v2
trailing-zero payload truncation rule.
- UDP and POSIX-serial transports behind a single async Transport trait;
the run loop owns transport open with bounded exponential backoff
(2 s serial / 5 s UDP cap) and a tokio::select! per-link read+write
loop.
- 1 Hz outbound HEARTBEAT scheduler + inbound-heartbeat watchdog that
fires LinkUp / LinkLost on a broadcast channel and feeds health detail
(connected, last_heartbeat_age_ms, signing_enabled, parse_errors).
mission_client (AZ-644):
- HTTPS GET /missions/{id} over rustls (no OpenSSL on the airframe).
- Bundled JSON Schema (crates/shared/contracts/mission-schema.json,
draft-07, additionalProperties:false) validates every response;
schema-invalid bodies surface as FetchError::SchemaInvalid with a
1 KiB sample of the raw body for offline analysis.
- Transient failures (timeout, 5xx, 429) retry with bounded exponential
backoff up to MissionClientOptions.max_attempts (default 5); permanent
failures (4xx, malformed URL) abort immediately.
- Health surface mirrors AC-1's contract: last_fetch_ts,
fetch_errors_total, schema_version, connection_state.
Caught and fixed before commit (NOT a code-review finding — caught by
the unit test that hand-computed CRC("123456789")): the hand-rolled
X.25 CRC accumulator was operating in u16 throughout. The MAVLink C
reference declares `tmp` as uint8_t, which silently truncates the
shifted-in bits. Round-trip tests passed (encoder and decoder shared
the bug); a real MAVLink peer would have rejected every frame. Fixed
by mirroring the C reference: `let mut tmp: u8 = …; tmp ^= tmp.wrapping_shl(4);`.
Added a regression test asserting CRC("123456789") == 0x6F91 against
pymavlink's reference value (NOT the textbook 0x29B1 — MAVLink uses a
byte-wise variant, not the bit-reflected CCITT).
AC verification (full detail in
_docs/03_implementation/batch_02_cycle1_report.md):
AZ-641: AC-1 + AC-3 + AC-4 verified via UDP loopback integration tests;
AC-2 (serial) requires a socat pty pair and runs in the SITL/CI
tier (test exists as #[ignore]-marked stub).
AZ-642: AC-1 + AC-2 + AC-3 verified via exhaustive codec round-trip and
decoder negative-path tests; AC-4 (SITL round-trip) requires
ArduPilot SITL — the CRC fix above means the codec is now
wire-correct, ready for the sitl-conformance Woodpecker stage.
AZ-644: all four ACs verified via wiremock-driven integration tests.
Workspace gates green:
- cargo check --workspace clean
- cargo check --workspace --no-default-features clean
- cargo fmt --all -- --check clean
- cargo clippy --workspace --all-targets -- -D warnings clean
- cargo test --workspace pass (1 expected ignore)
Layering invariants from module-layout.md hold: mavlink_layer and
mission_client are Layer 2 actors importing only `shared`; no sibling
Layer-2 imports; MavlinkHandle implements shared::contracts::MavlinkSink.
Jira: AZ-641, AZ-642, AZ-644 transitioned To Do → In Progress at batch
start; the matching In Testing transitions follow this commit.
Co-authored-by: Cursor <cursoragent@cursor.com>
936 lines
29 KiB
Rust
936 lines
29 KiB
Rust
//! Typed representations of every message in the §7.7 surface.
|
|
//!
|
|
//! For each message:
|
|
//! - `MSG_ID` is the MAVLink id used in the 3-byte msgid field.
|
|
//! - `CRC_EXTRA` is the message-specific crc-extra byte fed into the X.25 CRC.
|
|
//! - `SIZE` is the full payload length (before trailing-zero truncation).
|
|
//! - `encode_payload` writes the payload **at its full length**; trailing-zero
|
|
//! truncation is the framer's job.
|
|
//! - `decode_payload` accepts the **possibly truncated** payload and zero-pads
|
|
//! it back to `SIZE` before reading.
|
|
//!
|
|
//! Field ordering inside each payload follows the MAVLink size-sorted rule
|
|
//! (descending C primitive size, definition order on ties). This is the
|
|
//! invariant the [`CRC_EXTRA`] values were generated against; reordering any
|
|
//! field silently breaks wire compatibility with every peer.
|
|
|
|
use super::MavlinkParseError;
|
|
|
|
/// The codec-supported §7.7 surface as a single typed enum.
|
|
///
|
|
/// Adding a new variant requires explicit design-review notes per
|
|
/// `architecture.md §7.7`.
|
|
#[derive(Debug, Clone, PartialEq)]
|
|
pub enum MavlinkMessage {
|
|
Heartbeat(Heartbeat),
|
|
SysStatus(SysStatus),
|
|
SetMode(SetMode),
|
|
Attitude(Attitude),
|
|
GlobalPositionInt(GlobalPositionInt),
|
|
MissionSetCurrent(MissionSetCurrent),
|
|
MissionCurrent(MissionCurrent),
|
|
MissionCount(MissionCount),
|
|
MissionClearAll(MissionClearAll),
|
|
MissionItemReached(MissionItemReached),
|
|
MissionAck(MissionAck),
|
|
MissionRequestInt(MissionRequestInt),
|
|
MissionItemInt(MissionItemInt),
|
|
CommandLong(CommandLong),
|
|
CommandAck(CommandAck),
|
|
ExtendedSysState(ExtendedSysState),
|
|
StatusText(StatusText),
|
|
}
|
|
|
|
impl MavlinkMessage {
|
|
pub fn msg_id(&self) -> u32 {
|
|
match self {
|
|
Self::Heartbeat(_) => Heartbeat::MSG_ID,
|
|
Self::SysStatus(_) => SysStatus::MSG_ID,
|
|
Self::SetMode(_) => SetMode::MSG_ID,
|
|
Self::Attitude(_) => Attitude::MSG_ID,
|
|
Self::GlobalPositionInt(_) => GlobalPositionInt::MSG_ID,
|
|
Self::MissionSetCurrent(_) => MissionSetCurrent::MSG_ID,
|
|
Self::MissionCurrent(_) => MissionCurrent::MSG_ID,
|
|
Self::MissionCount(_) => MissionCount::MSG_ID,
|
|
Self::MissionClearAll(_) => MissionClearAll::MSG_ID,
|
|
Self::MissionItemReached(_) => MissionItemReached::MSG_ID,
|
|
Self::MissionAck(_) => MissionAck::MSG_ID,
|
|
Self::MissionRequestInt(_) => MissionRequestInt::MSG_ID,
|
|
Self::MissionItemInt(_) => MissionItemInt::MSG_ID,
|
|
Self::CommandLong(_) => CommandLong::MSG_ID,
|
|
Self::CommandAck(_) => CommandAck::MSG_ID,
|
|
Self::ExtendedSysState(_) => ExtendedSysState::MSG_ID,
|
|
Self::StatusText(_) => StatusText::MSG_ID,
|
|
}
|
|
}
|
|
|
|
pub fn crc_extra(&self) -> u8 {
|
|
match self {
|
|
Self::Heartbeat(_) => Heartbeat::CRC_EXTRA,
|
|
Self::SysStatus(_) => SysStatus::CRC_EXTRA,
|
|
Self::SetMode(_) => SetMode::CRC_EXTRA,
|
|
Self::Attitude(_) => Attitude::CRC_EXTRA,
|
|
Self::GlobalPositionInt(_) => GlobalPositionInt::CRC_EXTRA,
|
|
Self::MissionSetCurrent(_) => MissionSetCurrent::CRC_EXTRA,
|
|
Self::MissionCurrent(_) => MissionCurrent::CRC_EXTRA,
|
|
Self::MissionCount(_) => MissionCount::CRC_EXTRA,
|
|
Self::MissionClearAll(_) => MissionClearAll::CRC_EXTRA,
|
|
Self::MissionItemReached(_) => MissionItemReached::CRC_EXTRA,
|
|
Self::MissionAck(_) => MissionAck::CRC_EXTRA,
|
|
Self::MissionRequestInt(_) => MissionRequestInt::CRC_EXTRA,
|
|
Self::MissionItemInt(_) => MissionItemInt::CRC_EXTRA,
|
|
Self::CommandLong(_) => CommandLong::CRC_EXTRA,
|
|
Self::CommandAck(_) => CommandAck::CRC_EXTRA,
|
|
Self::ExtendedSysState(_) => ExtendedSysState::CRC_EXTRA,
|
|
Self::StatusText(_) => StatusText::CRC_EXTRA,
|
|
}
|
|
}
|
|
|
|
pub fn encode_payload(&self, buf: &mut Vec<u8>) {
|
|
match self {
|
|
Self::Heartbeat(m) => m.encode(buf),
|
|
Self::SysStatus(m) => m.encode(buf),
|
|
Self::SetMode(m) => m.encode(buf),
|
|
Self::Attitude(m) => m.encode(buf),
|
|
Self::GlobalPositionInt(m) => m.encode(buf),
|
|
Self::MissionSetCurrent(m) => m.encode(buf),
|
|
Self::MissionCurrent(m) => m.encode(buf),
|
|
Self::MissionCount(m) => m.encode(buf),
|
|
Self::MissionClearAll(m) => m.encode(buf),
|
|
Self::MissionItemReached(m) => m.encode(buf),
|
|
Self::MissionAck(m) => m.encode(buf),
|
|
Self::MissionRequestInt(m) => m.encode(buf),
|
|
Self::MissionItemInt(m) => m.encode(buf),
|
|
Self::CommandLong(m) => m.encode(buf),
|
|
Self::CommandAck(m) => m.encode(buf),
|
|
Self::ExtendedSysState(m) => m.encode(buf),
|
|
Self::StatusText(m) => m.encode(buf),
|
|
}
|
|
}
|
|
|
|
pub fn decode(msg_id: u32, payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
match msg_id {
|
|
Heartbeat::MSG_ID => Ok(Self::Heartbeat(Heartbeat::decode(payload)?)),
|
|
SysStatus::MSG_ID => Ok(Self::SysStatus(SysStatus::decode(payload)?)),
|
|
SetMode::MSG_ID => Ok(Self::SetMode(SetMode::decode(payload)?)),
|
|
Attitude::MSG_ID => Ok(Self::Attitude(Attitude::decode(payload)?)),
|
|
GlobalPositionInt::MSG_ID => {
|
|
Ok(Self::GlobalPositionInt(GlobalPositionInt::decode(payload)?))
|
|
}
|
|
MissionSetCurrent::MSG_ID => {
|
|
Ok(Self::MissionSetCurrent(MissionSetCurrent::decode(payload)?))
|
|
}
|
|
MissionCurrent::MSG_ID => Ok(Self::MissionCurrent(MissionCurrent::decode(payload)?)),
|
|
MissionCount::MSG_ID => Ok(Self::MissionCount(MissionCount::decode(payload)?)),
|
|
MissionClearAll::MSG_ID => Ok(Self::MissionClearAll(MissionClearAll::decode(payload)?)),
|
|
MissionItemReached::MSG_ID => Ok(Self::MissionItemReached(MissionItemReached::decode(
|
|
payload,
|
|
)?)),
|
|
MissionAck::MSG_ID => Ok(Self::MissionAck(MissionAck::decode(payload)?)),
|
|
MissionRequestInt::MSG_ID => {
|
|
Ok(Self::MissionRequestInt(MissionRequestInt::decode(payload)?))
|
|
}
|
|
MissionItemInt::MSG_ID => Ok(Self::MissionItemInt(MissionItemInt::decode(payload)?)),
|
|
CommandLong::MSG_ID => Ok(Self::CommandLong(CommandLong::decode(payload)?)),
|
|
CommandAck::MSG_ID => Ok(Self::CommandAck(CommandAck::decode(payload)?)),
|
|
ExtendedSysState::MSG_ID => {
|
|
Ok(Self::ExtendedSysState(ExtendedSysState::decode(payload)?))
|
|
}
|
|
StatusText::MSG_ID => Ok(Self::StatusText(StatusText::decode(payload)?)),
|
|
other => Err(MavlinkParseError::UnknownMessageId(other)),
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Resolve the message-specific `crc_extra` for an inbound msg id; returns
|
|
/// `None` for ids outside the §7.7 surface.
|
|
pub fn crc_extra_for_id(msg_id: u32) -> Option<u8> {
|
|
Some(match msg_id {
|
|
Heartbeat::MSG_ID => Heartbeat::CRC_EXTRA,
|
|
SysStatus::MSG_ID => SysStatus::CRC_EXTRA,
|
|
SetMode::MSG_ID => SetMode::CRC_EXTRA,
|
|
Attitude::MSG_ID => Attitude::CRC_EXTRA,
|
|
GlobalPositionInt::MSG_ID => GlobalPositionInt::CRC_EXTRA,
|
|
MissionSetCurrent::MSG_ID => MissionSetCurrent::CRC_EXTRA,
|
|
MissionCurrent::MSG_ID => MissionCurrent::CRC_EXTRA,
|
|
MissionCount::MSG_ID => MissionCount::CRC_EXTRA,
|
|
MissionClearAll::MSG_ID => MissionClearAll::CRC_EXTRA,
|
|
MissionItemReached::MSG_ID => MissionItemReached::CRC_EXTRA,
|
|
MissionAck::MSG_ID => MissionAck::CRC_EXTRA,
|
|
MissionRequestInt::MSG_ID => MissionRequestInt::CRC_EXTRA,
|
|
MissionItemInt::MSG_ID => MissionItemInt::CRC_EXTRA,
|
|
CommandLong::MSG_ID => CommandLong::CRC_EXTRA,
|
|
CommandAck::MSG_ID => CommandAck::CRC_EXTRA,
|
|
ExtendedSysState::MSG_ID => ExtendedSysState::CRC_EXTRA,
|
|
StatusText::MSG_ID => StatusText::CRC_EXTRA,
|
|
_ => return None,
|
|
})
|
|
}
|
|
|
|
// ===== helpers =====
|
|
|
|
#[inline]
|
|
fn need(payload: &[u8], required: usize) -> Result<(), MavlinkParseError> {
|
|
if payload.len() < required {
|
|
return Err(MavlinkParseError::TruncatedPayload {
|
|
have: payload.len(),
|
|
need: required,
|
|
});
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
/// Pad `payload` to `size` bytes with trailing zeros; MAVLink v2 truncates
|
|
/// trailing zeros on the wire and the decoder is required to re-extend.
|
|
fn padded(payload: &[u8], size: usize) -> [u8; 64] {
|
|
debug_assert!(size <= 64, "max single-message payload in §7.7 fits in 64B");
|
|
let mut buf = [0u8; 64];
|
|
let copy = payload.len().min(size);
|
|
buf[..copy].copy_from_slice(&payload[..copy]);
|
|
buf
|
|
}
|
|
|
|
#[inline]
|
|
fn read_u16(b: &[u8], at: usize) -> u16 {
|
|
u16::from_le_bytes([b[at], b[at + 1]])
|
|
}
|
|
|
|
#[inline]
|
|
fn read_i16(b: &[u8], at: usize) -> i16 {
|
|
i16::from_le_bytes([b[at], b[at + 1]])
|
|
}
|
|
|
|
#[inline]
|
|
fn read_u32(b: &[u8], at: usize) -> u32 {
|
|
u32::from_le_bytes([b[at], b[at + 1], b[at + 2], b[at + 3]])
|
|
}
|
|
|
|
#[inline]
|
|
fn read_i32(b: &[u8], at: usize) -> i32 {
|
|
i32::from_le_bytes([b[at], b[at + 1], b[at + 2], b[at + 3]])
|
|
}
|
|
|
|
#[inline]
|
|
fn read_f32(b: &[u8], at: usize) -> f32 {
|
|
f32::from_le_bytes([b[at], b[at + 1], b[at + 2], b[at + 3]])
|
|
}
|
|
|
|
// ===== HEARTBEAT (id 0, crc_extra 50, size 9) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct Heartbeat {
|
|
pub custom_mode: u32,
|
|
pub mavtype: u8,
|
|
pub autopilot: u8,
|
|
pub base_mode: u8,
|
|
pub system_status: u8,
|
|
pub mavlink_version: u8,
|
|
}
|
|
|
|
impl Heartbeat {
|
|
pub const MSG_ID: u32 = 0;
|
|
pub const CRC_EXTRA: u8 = 50;
|
|
pub const SIZE: usize = 9;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.extend_from_slice(&self.custom_mode.to_le_bytes());
|
|
buf.push(self.mavtype);
|
|
buf.push(self.autopilot);
|
|
buf.push(self.base_mode);
|
|
buf.push(self.system_status);
|
|
buf.push(self.mavlink_version);
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
custom_mode: read_u32(&b, 0),
|
|
mavtype: b[4],
|
|
autopilot: b[5],
|
|
base_mode: b[6],
|
|
system_status: b[7],
|
|
mavlink_version: b[8],
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== SYS_STATUS (id 1, crc_extra 124, size 31) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct SysStatus {
|
|
pub onboard_control_sensors_present: u32,
|
|
pub onboard_control_sensors_enabled: u32,
|
|
pub onboard_control_sensors_health: u32,
|
|
pub load: u16,
|
|
pub voltage_battery: u16,
|
|
pub current_battery: i16,
|
|
pub drop_rate_comm: u16,
|
|
pub errors_comm: u16,
|
|
pub errors_count1: u16,
|
|
pub errors_count2: u16,
|
|
pub errors_count3: u16,
|
|
pub errors_count4: u16,
|
|
pub battery_remaining: i8,
|
|
}
|
|
|
|
impl SysStatus {
|
|
pub const MSG_ID: u32 = 1;
|
|
pub const CRC_EXTRA: u8 = 124;
|
|
pub const SIZE: usize = 31;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.extend_from_slice(&self.onboard_control_sensors_present.to_le_bytes());
|
|
buf.extend_from_slice(&self.onboard_control_sensors_enabled.to_le_bytes());
|
|
buf.extend_from_slice(&self.onboard_control_sensors_health.to_le_bytes());
|
|
buf.extend_from_slice(&self.load.to_le_bytes());
|
|
buf.extend_from_slice(&self.voltage_battery.to_le_bytes());
|
|
buf.extend_from_slice(&self.current_battery.to_le_bytes());
|
|
buf.extend_from_slice(&self.drop_rate_comm.to_le_bytes());
|
|
buf.extend_from_slice(&self.errors_comm.to_le_bytes());
|
|
buf.extend_from_slice(&self.errors_count1.to_le_bytes());
|
|
buf.extend_from_slice(&self.errors_count2.to_le_bytes());
|
|
buf.extend_from_slice(&self.errors_count3.to_le_bytes());
|
|
buf.extend_from_slice(&self.errors_count4.to_le_bytes());
|
|
buf.push(self.battery_remaining as u8);
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
onboard_control_sensors_present: read_u32(&b, 0),
|
|
onboard_control_sensors_enabled: read_u32(&b, 4),
|
|
onboard_control_sensors_health: read_u32(&b, 8),
|
|
load: read_u16(&b, 12),
|
|
voltage_battery: read_u16(&b, 14),
|
|
current_battery: read_i16(&b, 16),
|
|
drop_rate_comm: read_u16(&b, 18),
|
|
errors_comm: read_u16(&b, 20),
|
|
errors_count1: read_u16(&b, 22),
|
|
errors_count2: read_u16(&b, 24),
|
|
errors_count3: read_u16(&b, 26),
|
|
errors_count4: read_u16(&b, 28),
|
|
battery_remaining: b[30] as i8,
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== SET_MODE (id 11, crc_extra 89, size 6) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct SetMode {
|
|
pub custom_mode: u32,
|
|
pub target_system: u8,
|
|
pub base_mode: u8,
|
|
}
|
|
|
|
impl SetMode {
|
|
pub const MSG_ID: u32 = 11;
|
|
pub const CRC_EXTRA: u8 = 89;
|
|
pub const SIZE: usize = 6;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.extend_from_slice(&self.custom_mode.to_le_bytes());
|
|
buf.push(self.target_system);
|
|
buf.push(self.base_mode);
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
custom_mode: read_u32(&b, 0),
|
|
target_system: b[4],
|
|
base_mode: b[5],
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== ATTITUDE (id 30, crc_extra 39, size 28) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct Attitude {
|
|
pub time_boot_ms: u32,
|
|
pub roll: f32,
|
|
pub pitch: f32,
|
|
pub yaw: f32,
|
|
pub rollspeed: f32,
|
|
pub pitchspeed: f32,
|
|
pub yawspeed: f32,
|
|
}
|
|
|
|
impl Attitude {
|
|
pub const MSG_ID: u32 = 30;
|
|
pub const CRC_EXTRA: u8 = 39;
|
|
pub const SIZE: usize = 28;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.extend_from_slice(&self.time_boot_ms.to_le_bytes());
|
|
buf.extend_from_slice(&self.roll.to_le_bytes());
|
|
buf.extend_from_slice(&self.pitch.to_le_bytes());
|
|
buf.extend_from_slice(&self.yaw.to_le_bytes());
|
|
buf.extend_from_slice(&self.rollspeed.to_le_bytes());
|
|
buf.extend_from_slice(&self.pitchspeed.to_le_bytes());
|
|
buf.extend_from_slice(&self.yawspeed.to_le_bytes());
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
time_boot_ms: read_u32(&b, 0),
|
|
roll: read_f32(&b, 4),
|
|
pitch: read_f32(&b, 8),
|
|
yaw: read_f32(&b, 12),
|
|
rollspeed: read_f32(&b, 16),
|
|
pitchspeed: read_f32(&b, 20),
|
|
yawspeed: read_f32(&b, 24),
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== GLOBAL_POSITION_INT (id 33, crc_extra 104, size 28) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct GlobalPositionInt {
|
|
pub time_boot_ms: u32,
|
|
pub lat_e7: i32,
|
|
pub lon_e7: i32,
|
|
pub alt_mm: i32,
|
|
pub relative_alt_mm: i32,
|
|
pub vx_cmps: i16,
|
|
pub vy_cmps: i16,
|
|
pub vz_cmps: i16,
|
|
pub hdg_cdeg: u16,
|
|
}
|
|
|
|
impl GlobalPositionInt {
|
|
pub const MSG_ID: u32 = 33;
|
|
pub const CRC_EXTRA: u8 = 104;
|
|
pub const SIZE: usize = 28;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.extend_from_slice(&self.time_boot_ms.to_le_bytes());
|
|
buf.extend_from_slice(&self.lat_e7.to_le_bytes());
|
|
buf.extend_from_slice(&self.lon_e7.to_le_bytes());
|
|
buf.extend_from_slice(&self.alt_mm.to_le_bytes());
|
|
buf.extend_from_slice(&self.relative_alt_mm.to_le_bytes());
|
|
buf.extend_from_slice(&self.vx_cmps.to_le_bytes());
|
|
buf.extend_from_slice(&self.vy_cmps.to_le_bytes());
|
|
buf.extend_from_slice(&self.vz_cmps.to_le_bytes());
|
|
buf.extend_from_slice(&self.hdg_cdeg.to_le_bytes());
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
time_boot_ms: read_u32(&b, 0),
|
|
lat_e7: read_i32(&b, 4),
|
|
lon_e7: read_i32(&b, 8),
|
|
alt_mm: read_i32(&b, 12),
|
|
relative_alt_mm: read_i32(&b, 16),
|
|
vx_cmps: read_i16(&b, 20),
|
|
vy_cmps: read_i16(&b, 22),
|
|
vz_cmps: read_i16(&b, 24),
|
|
hdg_cdeg: read_u16(&b, 26),
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== MISSION_SET_CURRENT (id 41, crc_extra 28, size 4) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct MissionSetCurrent {
|
|
pub seq: u16,
|
|
pub target_system: u8,
|
|
pub target_component: u8,
|
|
}
|
|
|
|
impl MissionSetCurrent {
|
|
pub const MSG_ID: u32 = 41;
|
|
pub const CRC_EXTRA: u8 = 28;
|
|
pub const SIZE: usize = 4;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.extend_from_slice(&self.seq.to_le_bytes());
|
|
buf.push(self.target_system);
|
|
buf.push(self.target_component);
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
seq: read_u16(&b, 0),
|
|
target_system: b[2],
|
|
target_component: b[3],
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== MISSION_CURRENT (id 42, crc_extra 28, size 2) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct MissionCurrent {
|
|
pub seq: u16,
|
|
}
|
|
|
|
impl MissionCurrent {
|
|
pub const MSG_ID: u32 = 42;
|
|
pub const CRC_EXTRA: u8 = 28;
|
|
pub const SIZE: usize = 2;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.extend_from_slice(&self.seq.to_le_bytes());
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
seq: read_u16(&b, 0),
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== MISSION_COUNT (id 44, crc_extra 221, size 4) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct MissionCount {
|
|
pub count: u16,
|
|
pub target_system: u8,
|
|
pub target_component: u8,
|
|
}
|
|
|
|
impl MissionCount {
|
|
pub const MSG_ID: u32 = 44;
|
|
pub const CRC_EXTRA: u8 = 221;
|
|
pub const SIZE: usize = 4;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.extend_from_slice(&self.count.to_le_bytes());
|
|
buf.push(self.target_system);
|
|
buf.push(self.target_component);
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
count: read_u16(&b, 0),
|
|
target_system: b[2],
|
|
target_component: b[3],
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== MISSION_CLEAR_ALL (id 45, crc_extra 232, size 2) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct MissionClearAll {
|
|
pub target_system: u8,
|
|
pub target_component: u8,
|
|
}
|
|
|
|
impl MissionClearAll {
|
|
pub const MSG_ID: u32 = 45;
|
|
pub const CRC_EXTRA: u8 = 232;
|
|
pub const SIZE: usize = 2;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.push(self.target_system);
|
|
buf.push(self.target_component);
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
target_system: b[0],
|
|
target_component: b[1],
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== MISSION_ITEM_REACHED (id 46, crc_extra 11, size 2) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct MissionItemReached {
|
|
pub seq: u16,
|
|
}
|
|
|
|
impl MissionItemReached {
|
|
pub const MSG_ID: u32 = 46;
|
|
pub const CRC_EXTRA: u8 = 11;
|
|
pub const SIZE: usize = 2;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.extend_from_slice(&self.seq.to_le_bytes());
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
seq: read_u16(&b, 0),
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== MISSION_ACK (id 47, crc_extra 153, size 3) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct MissionAck {
|
|
pub target_system: u8,
|
|
pub target_component: u8,
|
|
pub mission_result: u8,
|
|
}
|
|
|
|
impl MissionAck {
|
|
pub const MSG_ID: u32 = 47;
|
|
pub const CRC_EXTRA: u8 = 153;
|
|
pub const SIZE: usize = 3;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.push(self.target_system);
|
|
buf.push(self.target_component);
|
|
buf.push(self.mission_result);
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
target_system: b[0],
|
|
target_component: b[1],
|
|
mission_result: b[2],
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== MISSION_REQUEST_INT (id 51, crc_extra 38, size 4) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct MissionRequestInt {
|
|
pub seq: u16,
|
|
pub target_system: u8,
|
|
pub target_component: u8,
|
|
}
|
|
|
|
impl MissionRequestInt {
|
|
pub const MSG_ID: u32 = 51;
|
|
pub const CRC_EXTRA: u8 = 38;
|
|
pub const SIZE: usize = 4;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.extend_from_slice(&self.seq.to_le_bytes());
|
|
buf.push(self.target_system);
|
|
buf.push(self.target_component);
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
seq: read_u16(&b, 0),
|
|
target_system: b[2],
|
|
target_component: b[3],
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== MISSION_ITEM_INT (id 73, crc_extra 38, size 37) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct MissionItemInt {
|
|
pub param1: f32,
|
|
pub param2: f32,
|
|
pub param3: f32,
|
|
pub param4: f32,
|
|
pub x: i32,
|
|
pub y: i32,
|
|
pub z: f32,
|
|
pub seq: u16,
|
|
pub command: u16,
|
|
pub target_system: u8,
|
|
pub target_component: u8,
|
|
pub frame: u8,
|
|
pub current: u8,
|
|
pub autocontinue: u8,
|
|
}
|
|
|
|
impl MissionItemInt {
|
|
pub const MSG_ID: u32 = 73;
|
|
pub const CRC_EXTRA: u8 = 38;
|
|
pub const SIZE: usize = 37;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.extend_from_slice(&self.param1.to_le_bytes());
|
|
buf.extend_from_slice(&self.param2.to_le_bytes());
|
|
buf.extend_from_slice(&self.param3.to_le_bytes());
|
|
buf.extend_from_slice(&self.param4.to_le_bytes());
|
|
buf.extend_from_slice(&self.x.to_le_bytes());
|
|
buf.extend_from_slice(&self.y.to_le_bytes());
|
|
buf.extend_from_slice(&self.z.to_le_bytes());
|
|
buf.extend_from_slice(&self.seq.to_le_bytes());
|
|
buf.extend_from_slice(&self.command.to_le_bytes());
|
|
buf.push(self.target_system);
|
|
buf.push(self.target_component);
|
|
buf.push(self.frame);
|
|
buf.push(self.current);
|
|
buf.push(self.autocontinue);
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
param1: read_f32(&b, 0),
|
|
param2: read_f32(&b, 4),
|
|
param3: read_f32(&b, 8),
|
|
param4: read_f32(&b, 12),
|
|
x: read_i32(&b, 16),
|
|
y: read_i32(&b, 20),
|
|
z: read_f32(&b, 24),
|
|
seq: read_u16(&b, 28),
|
|
command: read_u16(&b, 30),
|
|
target_system: b[32],
|
|
target_component: b[33],
|
|
frame: b[34],
|
|
current: b[35],
|
|
autocontinue: b[36],
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== COMMAND_LONG (id 76, crc_extra 152, size 33) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct CommandLong {
|
|
pub param1: f32,
|
|
pub param2: f32,
|
|
pub param3: f32,
|
|
pub param4: f32,
|
|
pub param5: f32,
|
|
pub param6: f32,
|
|
pub param7: f32,
|
|
pub command: u16,
|
|
pub target_system: u8,
|
|
pub target_component: u8,
|
|
pub confirmation: u8,
|
|
}
|
|
|
|
impl CommandLong {
|
|
pub const MSG_ID: u32 = 76;
|
|
pub const CRC_EXTRA: u8 = 152;
|
|
pub const SIZE: usize = 33;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.extend_from_slice(&self.param1.to_le_bytes());
|
|
buf.extend_from_slice(&self.param2.to_le_bytes());
|
|
buf.extend_from_slice(&self.param3.to_le_bytes());
|
|
buf.extend_from_slice(&self.param4.to_le_bytes());
|
|
buf.extend_from_slice(&self.param5.to_le_bytes());
|
|
buf.extend_from_slice(&self.param6.to_le_bytes());
|
|
buf.extend_from_slice(&self.param7.to_le_bytes());
|
|
buf.extend_from_slice(&self.command.to_le_bytes());
|
|
buf.push(self.target_system);
|
|
buf.push(self.target_component);
|
|
buf.push(self.confirmation);
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
param1: read_f32(&b, 0),
|
|
param2: read_f32(&b, 4),
|
|
param3: read_f32(&b, 8),
|
|
param4: read_f32(&b, 12),
|
|
param5: read_f32(&b, 16),
|
|
param6: read_f32(&b, 20),
|
|
param7: read_f32(&b, 24),
|
|
command: read_u16(&b, 28),
|
|
target_system: b[30],
|
|
target_component: b[31],
|
|
confirmation: b[32],
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== COMMAND_ACK (id 77, crc_extra 143, size 3) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct CommandAck {
|
|
pub command: u16,
|
|
pub result: u8,
|
|
}
|
|
|
|
impl CommandAck {
|
|
pub const MSG_ID: u32 = 77;
|
|
pub const CRC_EXTRA: u8 = 143;
|
|
pub const SIZE: usize = 3;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.extend_from_slice(&self.command.to_le_bytes());
|
|
buf.push(self.result);
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
command: read_u16(&b, 0),
|
|
result: b[2],
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== EXTENDED_SYS_STATE (id 245, crc_extra 130, size 2) =====
|
|
|
|
#[derive(Debug, Clone, Copy, PartialEq)]
|
|
pub struct ExtendedSysState {
|
|
pub vtol_state: u8,
|
|
pub landed_state: u8,
|
|
}
|
|
|
|
impl ExtendedSysState {
|
|
pub const MSG_ID: u32 = 245;
|
|
pub const CRC_EXTRA: u8 = 130;
|
|
pub const SIZE: usize = 2;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.push(self.vtol_state);
|
|
buf.push(self.landed_state);
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
let b = padded(payload, Self::SIZE);
|
|
Ok(Self {
|
|
vtol_state: b[0],
|
|
landed_state: b[1],
|
|
})
|
|
}
|
|
}
|
|
|
|
// ===== STATUSTEXT (id 253, crc_extra 83, size 51) =====
|
|
|
|
#[derive(Debug, Clone, PartialEq)]
|
|
pub struct StatusText {
|
|
pub severity: u8,
|
|
/// 50-byte null-padded ASCII; values beyond the first NUL are ignored on
|
|
/// decode. Encoding zero-pads to 50 bytes.
|
|
pub text: [u8; 50],
|
|
}
|
|
|
|
impl StatusText {
|
|
pub const MSG_ID: u32 = 253;
|
|
pub const CRC_EXTRA: u8 = 83;
|
|
pub const SIZE: usize = 51;
|
|
|
|
pub fn encode(&self, buf: &mut Vec<u8>) {
|
|
buf.push(self.severity);
|
|
buf.extend_from_slice(&self.text);
|
|
}
|
|
|
|
pub fn decode(payload: &[u8]) -> Result<Self, MavlinkParseError> {
|
|
need(payload, 1)?;
|
|
let mut text = [0u8; 50];
|
|
let body_len = payload.len() - 1;
|
|
let copy = body_len.min(50);
|
|
text[..copy].copy_from_slice(&payload[1..1 + copy]);
|
|
Ok(Self {
|
|
severity: payload[0],
|
|
text,
|
|
})
|
|
}
|
|
|
|
/// Build a `StatusText` from a UTF-8 string, truncating to 50 bytes and
|
|
/// zero-padding the rest.
|
|
pub fn from_str(severity: u8, msg: &str) -> Self {
|
|
let bytes = msg.as_bytes();
|
|
let mut text = [0u8; 50];
|
|
let copy = bytes.len().min(50);
|
|
text[..copy].copy_from_slice(&bytes[..copy]);
|
|
Self { severity, text }
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
#[test]
|
|
fn heartbeat_round_trips() {
|
|
// Arrange
|
|
let m = Heartbeat {
|
|
custom_mode: 0xDEADBEEF,
|
|
mavtype: 2,
|
|
autopilot: 3,
|
|
base_mode: 0x81,
|
|
system_status: 4,
|
|
mavlink_version: 3,
|
|
};
|
|
|
|
// Act
|
|
let mut buf = Vec::new();
|
|
m.encode(&mut buf);
|
|
let decoded = Heartbeat::decode(&buf).unwrap();
|
|
|
|
// Assert
|
|
assert_eq!(buf.len(), Heartbeat::SIZE);
|
|
assert_eq!(decoded, m);
|
|
}
|
|
|
|
#[test]
|
|
fn command_long_round_trips() {
|
|
// Arrange
|
|
let m = CommandLong {
|
|
param1: 1.5,
|
|
param2: 2.25,
|
|
param3: -3.0,
|
|
param4: f32::NAN,
|
|
param5: 50.123,
|
|
param6: -42.42,
|
|
param7: 100.0,
|
|
command: 20, // MAV_CMD_NAV_RETURN_TO_LAUNCH
|
|
target_system: 1,
|
|
target_component: 1,
|
|
confirmation: 0,
|
|
};
|
|
|
|
// Act
|
|
let mut buf = Vec::new();
|
|
m.encode(&mut buf);
|
|
let decoded = CommandLong::decode(&buf).unwrap();
|
|
|
|
// Assert: NaN compares unequal so compare bit pattern manually
|
|
assert_eq!(buf.len(), CommandLong::SIZE);
|
|
assert_eq!(decoded.param1, m.param1);
|
|
assert!(decoded.param4.is_nan());
|
|
assert_eq!(decoded.command, m.command);
|
|
assert_eq!(decoded.confirmation, m.confirmation);
|
|
}
|
|
|
|
#[test]
|
|
fn statustext_truncates_long_string() {
|
|
// Arrange
|
|
let long = "x".repeat(100);
|
|
|
|
// Act
|
|
let m = StatusText::from_str(6, &long);
|
|
|
|
// Assert
|
|
assert_eq!(m.text[..50], [b'x'; 50][..]);
|
|
}
|
|
|
|
#[test]
|
|
fn decode_truncated_heartbeat_zero_pads() {
|
|
// Arrange: a HEARTBEAT payload trimmed of its trailing mavlink_version byte
|
|
let mut buf = Vec::new();
|
|
Heartbeat {
|
|
custom_mode: 7,
|
|
mavtype: 2,
|
|
autopilot: 3,
|
|
base_mode: 0,
|
|
system_status: 4,
|
|
mavlink_version: 3,
|
|
}
|
|
.encode(&mut buf);
|
|
let truncated = &buf[..Heartbeat::SIZE - 1];
|
|
|
|
// Act
|
|
let decoded = Heartbeat::decode(truncated).unwrap();
|
|
|
|
// Assert: the trimmed byte is read as zero (MAVLink v2 trailing-zero rule).
|
|
assert_eq!(decoded.mavlink_version, 0);
|
|
}
|
|
}
|