turret 0.1.3

MAVLink Gimbal Manager and CLI for STorM32 RC Commands gimbals
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102

//! Per-message MAVLink dispatch: every UDP datagram parsed into a
//! [`MavMessage`] lands in [`handle_mavlink_message`], which fans out to
//! one of the per-type handlers in the submodules below. All inbound
//! logic — primary-control gating, addressing, frame-flag clamping,
//! ACK shape, rate→position integration — lives under this module so
//! the periodic loops in [`super::loops`] don't see any of it.
//!
//! Submodule layout:
//!
//! - [`common`] — shared addressing / clamping / frame-transform /
//!   arbitrate-then-dispatch helpers, plus the addressing tests.
//! - [`set`] — `GIMBAL_MANAGER_SET_ATTITUDE` / `_PITCHYAW` /
//!   `_MANUAL_CONTROL` handlers.
//! - [`cmd_long`] — `COMMAND_LONG` dispatch and per-command handlers
//!   (PITCHYAW, CONFIGURE, REQUEST_MESSAGE).
//! - [`rate`] — rate→position integrator shared by SET_* and
//!   COMMAND_LONG rate paths.

#[path = "inbound/cmd_long.rs"]
mod cmd_long;
#[path = "inbound/common.rs"]
mod common;
#[path = "inbound/rate.rs"]
mod rate;
#[path = "inbound/set.rs"]
mod set;

use super::build::send_gimbal_manager_information;
use crate::daemon::mavlink_manager::math::quaternion_wxyz_to_euler_deg;
use crate::daemon::mavlink_manager::RxCtx;
use crate::daemon::state::VehicleState;
use mavlink::ardupilotmega::{MavMessage, AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_DATA};
use std::net::SocketAddr;
use tracing::debug;

/// Dispatch a parsed inbound `MavMessage` to a per-type handler.
#[tracing::instrument(level = "debug", skip(msg, ctx))]
pub(in crate::daemon::mavlink_manager) async fn handle_mavlink_message(
    sender_sysid: u8,
    sender_compid: u8,
    src_addr: SocketAddr,
    msg: MavMessage,
    ctx: &RxCtx<'_>,
) {
    match msg {
        MavMessage::GIMBAL_MANAGER_SET_ATTITUDE(m) => {
            set::on_set_attitude(sender_sysid, sender_compid, m, ctx).await;
        }
        MavMessage::GIMBAL_MANAGER_SET_PITCHYAW(m) => {
            set::on_set_pitchyaw(sender_sysid, sender_compid, m, ctx).await;
        }
        MavMessage::GIMBAL_MANAGER_SET_MANUAL_CONTROL(m) => {
            set::on_set_manual(sender_sysid, sender_compid, m, ctx).await;
        }
        MavMessage::GIMBAL_MANAGER_INFORMATION(_) => {
            debug!("Received GIMBAL_MANAGER_INFORMATION request");
            send_gimbal_manager_information(
                ctx.socket,
                src_addr,
                ctx.config,
                ctx.seq,
                ctx.start_time,
            )
            .await;
        }
        MavMessage::COMMAND_LONG(cmd) => {
            cmd_long::on_command_long(sender_sysid, sender_compid, src_addr, cmd, ctx).await;
        }
        MavMessage::AUTOPILOT_STATE_FOR_GIMBAL_DEVICE(m) => {
            on_autopilot_state(m, ctx);
        }
        _ => debug!("Ignoring MAVLink message type: {:?}", msg),
    }
}

/// Ingest the autopilot's full vehicle state from
/// `AUTOPILOT_STATE_FOR_GIMBAL_DEVICE`. The quaternion is required (we
/// drop the whole packet if it's NaN — without an attitude estimate
/// the rest of the message is operationally useless to a gimbal). The
/// scalar fields preserve their NaN sentinels per the spec ("NaN if
/// unknown" for velocity / feed-forward yaw rate); the typed
/// [`VehicleState::yaw_rate_deg_s`] uses `Option<f32>` so consumers
/// can branch on validity without re-checking NaN.
fn on_autopilot_state(m: AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_DATA, ctx: &RxCtx<'_>) {
    if m.q.iter().any(|c| c.is_nan()) {
        debug!("AUTOPILOT_STATE_FOR_GIMBAL_DEVICE with NaN quaternion — ignoring");
        return;
    }
    let (pitch_deg, roll_deg, yaw_deg) = quaternion_wxyz_to_euler_deg(m.q);
    let yaw_rate_deg_s = if m.feed_forward_angular_velocity_z.is_nan() {
        None
    } else {
        Some(m.feed_forward_angular_velocity_z.to_degrees())
    };
    ctx.state.update_vehicle_state(VehicleState {
        pitch_deg,
        roll_deg,
        yaw_deg,
        yaw_rate_deg_s,
        ned_velocity_m_s: (m.vx, m.vy, m.vz),
    });
}