autopilot/crates/mavlink_layer/tests/codec_round_trip.rs

//! AZ-642 AC-1: every supported message round-trips through the encoder and
//! decoder with byte-equal fields.

use mavlink_layer::{
    Attitude, CommandAck, CommandLong, Decoder, DecoderEvent, Encoder, ExtendedSysState,
    GlobalPositionInt, Heartbeat, MavlinkMessage, MissionAck, MissionClearAll, MissionCount,
    MissionCurrent, MissionItemInt, MissionItemReached, MissionRequestInt, MissionSetCurrent,
    SetMode, StatusText, SysStatus,
};

fn all_messages() -> Vec<MavlinkMessage> {
    vec![
        MavlinkMessage::Heartbeat(Heartbeat {
            custom_mode: 0xDEADBEEF,
            mavtype: 2,
            autopilot: 3,
            base_mode: 0x81,
            system_status: 4,
            mavlink_version: 3,
        }),
        MavlinkMessage::SysStatus(SysStatus {
            onboard_control_sensors_present: 0x1234_5678,
            onboard_control_sensors_enabled: 0xAAAA_BBBB,
            onboard_control_sensors_health: 0xCCCC_DDDD,
            load: 543,
            voltage_battery: 16000,
            current_battery: -250,
            drop_rate_comm: 12,
            errors_comm: 1,
            errors_count1: 2,
            errors_count2: 3,
            errors_count3: 4,
            errors_count4: 5,
            battery_remaining: 75,
        }),
        MavlinkMessage::SetMode(SetMode {
            custom_mode: 7,
            target_system: 1,
            base_mode: 0x81,
        }),
        MavlinkMessage::Attitude(Attitude {
            time_boot_ms: 12345,
            roll: 0.1,
            pitch: -0.05,
            yaw: std::f32::consts::FRAC_PI_2,
            rollspeed: 0.001,
            pitchspeed: -0.002,
            yawspeed: 0.0,
        }),
        MavlinkMessage::GlobalPositionInt(GlobalPositionInt {
            time_boot_ms: 99999,
            lat_e7: 503_456_789,
            lon_e7: 304_567_890,
            alt_mm: 12_345_678,
            relative_alt_mm: 5_000_000,
            vx_cmps: 100,
            vy_cmps: -50,
            vz_cmps: 25,
            hdg_cdeg: 18000,
        }),
        MavlinkMessage::MissionSetCurrent(MissionSetCurrent {
            seq: 7,
            target_system: 1,
            target_component: 1,
        }),
        MavlinkMessage::MissionCurrent(MissionCurrent { seq: 42 }),
        MavlinkMessage::MissionCount(MissionCount {
            count: 16,
            target_system: 1,
            target_component: 1,
        }),
        MavlinkMessage::MissionClearAll(MissionClearAll {
            target_system: 1,
            target_component: 1,
        }),
        MavlinkMessage::MissionItemReached(MissionItemReached { seq: 9 }),
        MavlinkMessage::MissionAck(MissionAck {
            target_system: 1,
            target_component: 1,
            mission_result: 0,
        }),
        MavlinkMessage::MissionRequestInt(MissionRequestInt {
            seq: 4,
            target_system: 1,
            target_component: 1,
        }),
        MavlinkMessage::MissionItemInt(MissionItemInt {
            param1: 1.0,
            param2: 2.0,
            param3: 3.0,
            param4: 4.0,
            x: 503_456_789,
            y: 304_567_890,
            z: 100.0,
            seq: 1,
            command: 16, // MAV_CMD_NAV_WAYPOINT
            target_system: 1,
            target_component: 1,
            frame: 3,
            current: 1,
            autocontinue: 1,
        }),
        MavlinkMessage::CommandLong(CommandLong {
            param1: 1.5,
            param2: 2.25,
            param3: -3.0,
            param4: 0.0,
            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,
        }),
        MavlinkMessage::CommandAck(CommandAck {
            command: 20,
            result: 0, // MAV_RESULT_ACCEPTED
        }),
        MavlinkMessage::ExtendedSysState(ExtendedSysState {
            vtol_state: 0,
            landed_state: 1,
        }),
        MavlinkMessage::StatusText(StatusText::from_str(6, "self-test ok")),
    ]
}

#[test]
fn every_supported_message_round_trips() {
    // Arrange
    let enc = Encoder::new(1, 191);
    let mut dec = Decoder::new();
    let originals = all_messages();

    // Act: encode every message, concatenate, decode in one stream.
    let mut stream = Vec::new();
    for m in &originals {
        stream.extend_from_slice(&enc.encode(m));
    }
    let events = dec.feed(&stream);

    // Assert
    assert_eq!(dec.errors.snapshot().total(), 0);
    let decoded: Vec<MavlinkMessage> = events
        .into_iter()
        .filter_map(|e| match e {
            DecoderEvent::Message { message, .. } => Some(message),
            _ => None,
        })
        .collect();
    assert_eq!(decoded.len(), originals.len());
    for (i, (got, want)) in decoded.iter().zip(originals.iter()).enumerate() {
        assert_eq!(got, want, "message {i} did not round-trip");
    }
}

#[test]
fn malformed_crc_drops_frame_and_counts_error() {
    // Arrange
    let enc = Encoder::new(1, 191);
    let m = MavlinkMessage::Heartbeat(Heartbeat {
        custom_mode: 0,
        mavtype: 2,
        autopilot: 3,
        base_mode: 0,
        system_status: 4,
        mavlink_version: 3,
    });
    let good = enc.encode(&m);
    let mut bad = good.clone();
    let last = bad.len() - 1;
    bad[last] ^= 0xAA; // corrupt the CRC byte

    // Act
    let mut dec = Decoder::new();
    let _bad_events = dec.feed(&bad);
    let good_events = dec.feed(&good);

    // Assert
    assert_eq!(dec.errors.snapshot().crc, 1);
    assert!(
        good_events
            .iter()
            .any(|e| matches!(e, DecoderEvent::Message { .. })),
        "decoder must resume parsing after a bad frame"
    );
}

#[test]
fn unknown_message_id_counts_not_fatal() {
    // Arrange: hand-build a frame with msg_id 999 (outside the §7.7 surface).
    let mut frame = vec![0xFD];
    frame.extend_from_slice(&[
        0, // payload_len
        0, 0, 0, // incompat, compat, seq
        1, 0xBE, // sysid, compid
        0xE7, 0x03, 0x00, // msg_id 999 LE
        0x00, 0x00, // bogus CRC
    ]);

    // Act
    let mut dec = Decoder::new();
    let events = dec.feed(&frame);

    // Assert
    assert!(events
        .iter()
        .any(|e| matches!(e, DecoderEvent::UnknownId { msg_id: 999, .. })));
    assert_eq!(dec.errors.snapshot().unknown_id, 1);
}