azaion/autopilot
1
0
Fork 0
mirror of https://github.com/azaion/autopilot.git synced 2026-06-22 09:51:09 +00:00
Code Issues Packages Projects Releases Wiki Activity

[AZ-641] [AZ-642] [AZ-644] mavlink transport + codec + mission pull

Browse Source 
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>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-19 12:29:49 +03:00
parent a1ce3a6903
commit 740bf37d76
33 changed files with 5293 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
crates/mavlink_layer/src/internal/codec/messages.rs
+935
View File
@@ -0,0 +1,935 @@
//! 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);
}
}