103 lines
3.0 KiB
Rust
103 lines
3.0 KiB
Rust
//! Narrow MAVLink arm/disarm command decoding.
|
|
//!
|
|
//! This module deliberately accepts no actuator-value or motor-test command.
|
|
//! A board must pass accepted requests to its single actuator-state owner; this
|
|
//! decoder has no access to output hardware.
|
|
|
|
use mavlink::Message;
|
|
|
|
use crate::{MavlinkFrame, message};
|
|
|
|
/// A requested change to the vehicle's arming state.
|
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
pub enum ArmRequest {
|
|
/// Request the armed-idle state.
|
|
Arm,
|
|
/// Request electrically inactive outputs.
|
|
Disarm,
|
|
}
|
|
|
|
/// Decodes a targeted MAVLink `MAV_CMD_COMPONENT_ARM_DISARM` command.
|
|
///
|
|
/// Only `param1` values exactly equal to `0.0` (disarm) or `1.0` (arm) are
|
|
/// accepted. Broadcast targets are intentionally rejected: an actuator command
|
|
/// must name this vehicle and its component explicitly.
|
|
pub fn decode_arm_request(
|
|
frame: &MavlinkFrame,
|
|
system_id: u8,
|
|
component_id: u8,
|
|
) -> Option<ArmRequest> {
|
|
let message = message::MavMessage::parse(
|
|
mavlink::MavlinkVersion::V2,
|
|
frame.message_id(),
|
|
frame.payload(),
|
|
)
|
|
.ok()?;
|
|
|
|
let message::MavMessage::COMMAND_LONG(command) = message else {
|
|
return None;
|
|
};
|
|
if command.command != message::MavCmd::MAV_CMD_COMPONENT_ARM_DISARM
|
|
|| command.target_system != system_id
|
|
|| command.target_component != component_id
|
|
{
|
|
return None;
|
|
}
|
|
|
|
match command.param1 {
|
|
0.0 => Some(ArmRequest::Disarm),
|
|
1.0 => Some(ArmRequest::Arm),
|
|
_ => None,
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::{ArmRequest, decode_arm_request};
|
|
use crate::{MavlinkFrame, message};
|
|
|
|
fn command(target_system: u8, target_component: u8, value: f32) -> MavlinkFrame {
|
|
let mut frame = MavlinkFrame::new();
|
|
frame.serialize_message_data(
|
|
message::MavHeader {
|
|
sequence: 0,
|
|
system_id: 42,
|
|
component_id: 7,
|
|
},
|
|
&message::COMMAND_LONG_DATA {
|
|
param1: value,
|
|
param2: 0.0,
|
|
param3: 0.0,
|
|
param4: 0.0,
|
|
param5: 0.0,
|
|
param6: 0.0,
|
|
param7: 0.0,
|
|
command: message::MavCmd::MAV_CMD_COMPONENT_ARM_DISARM,
|
|
target_system,
|
|
target_component,
|
|
confirmation: 0,
|
|
},
|
|
);
|
|
frame
|
|
}
|
|
|
|
#[test]
|
|
fn accepts_explicit_targeted_arm_and_disarm() {
|
|
assert_eq!(
|
|
decode_arm_request(&command(1, 1, 1.0), 1, 1),
|
|
Some(ArmRequest::Arm)
|
|
);
|
|
assert_eq!(
|
|
decode_arm_request(&command(1, 1, 0.0), 1, 1),
|
|
Some(ArmRequest::Disarm)
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn rejects_broadcast_wrong_target_and_invalid_parameter() {
|
|
assert_eq!(decode_arm_request(&command(0, 0, 1.0), 1, 1), None);
|
|
assert_eq!(decode_arm_request(&command(2, 1, 1.0), 1, 1), None);
|
|
assert_eq!(decode_arm_request(&command(1, 1, 0.5), 1, 1), None);
|
|
}
|
|
}
|