[AZ-666] [AZ-673] [AZ-648] ignored set + UDS VLM + mission FSM batch 5
ci/woodpecker/push/build-arm Pipeline failed

AZ-666 mapobjects_store:
- internal/ignored.rs (HashSet<(mgrs, class_group)> for O(1) suppression)
- internal/passes.rs (per-region PassTracker with observed-id set and
  end-of-pass removed-candidate sweep)
- Classification::Ignored wired into classify; apply_decline +
  is_ignored + pass_start + end_of_pass on MapObjectsStoreHandle
- new tests/ignored_and_sweep.rs (3 AC + 2 supplementary)

AZ-673 vlm_client:
- internal/peer_cred.rs (Linux SO_PEERCRED via libc getsockopt;
  PeerCredOutcome::SkippedNonLinux on macOS dev hosts per
  description.md §8)
- internal/prompt.rs (pre-send ROI size + format + prompt
  non-emptiness validation)
- internal/wire.rs (length-prefixed JSON envelope with base64 ROI)
- internal/uds_client.rs (tokio UnixStream client; bounded
  reconnect; hard-stop on peer-cred mismatch; per-request deadline)
- VlmClient with both eager (open/connect) and lazy (new) ctor
- workspace Cargo.toml: base64 + libc as workspace deps

AZ-648 mission_executor:
- internal/types.rs (Variant, MissionState, TransitionKey,
  Telemetry, TransitionEvent, StepOutcome)
- internal/driver.rs (MissionDriver trait + DriverError +
  DriverAction)
- internal/fsm.rs (variant-agnostic Transition + FsmCore + step_one
  with per-transition retry budget keyed by TransitionKey)
- internal/multirotor.rs + internal/fixed_wing.rs (typed transition
  tables; multirotor has Armed/TakeOff, fixed-wing parks in
  WaitAuto for operator AUTO)
- public API: MissionExecutor::run spawns the FSM task and returns
  a clone-safe MissionExecutorHandle (state, health, subscribe,
  paused_reason, retry_count)
- new tests/state_machine.rs (AC-1..AC-4 via ScriptedDriver fake;
  SITL conformance lands with AZ-649 telemetry forwarding)

Workspace: cargo fmt + clippy -D warnings clean; full
cargo test --workspace --all-features green (1 ignored = AZ-665
perf gate). Tasks moved todo/ → done/, autodev state set to batch
6 selection.

Refs: _docs/03_implementation/batch_05_cycle1_report.md
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-19 16:54:00 +03:00
parent 69c0629350
commit b5cc0c321c
30 changed files with 3343 additions and 111 deletions
@@ -0,0 +1,77 @@
//! Abstraction over the airframe-control surface used by the FSM.
//!
//! Production impl wraps `mavlink_layer::MavlinkHandle` (mission upload
//! sequence, `MAV_CMD_COMPONENT_ARM_DISARM`, `MAV_CMD_NAV_TAKEOFF`,
//! `MAV_CMD_DO_SET_MODE`, etc.). The wiring of the production impl
//! lands together with AZ-649 (telemetry forwarding) — AZ-648 only
//! commits to the trait and the in-process fake used by the AC tests.
//!
//! The trait is intentionally narrow: each method is one step the FSM
//! treats as atomic. `upload_mission` covers the entire `MISSION_CLEAR_ALL
//! → MISSION_COUNT → MISSION_ITEM_INT* → MISSION_ACK → MISSION_SET_CURRENT`
//! sequence so the FSM stays a pure transition driver and does not
//! own per-message MAVLink state.
use async_trait::async_trait;
use shared::models::mission::MissionWaypoint;
/// Errors a driver can return for a single FSM step.
#[derive(Debug, Clone, thiserror::Error)]
pub enum DriverError {
/// Airframe rejected the command (e.g., MissionAck with non-zero
/// `mission_result`, or CommandAck != ACCEPTED). The FSM retries.
#[error("airframe rejected: {0}")]
Rejected(String),
/// Deadline elapsed before the airframe responded. The FSM retries.
#[error("driver timeout after {ms} ms")]
Timeout { ms: u64 },
/// Transport / IO failure. The FSM retries.
#[error("driver transport: {0}")]
Transport(String),
}
/// Discriminator for the retry/health stats so the driver impl can
/// log which kind of action just failed.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum DriverAction {
Arm,
TakeOff,
UploadMission,
/// Used by the lost-link ladder (AZ-651) and by tests that
/// programmatically force the fixed-wing AUTO transition. AZ-648
/// itself routes `WaitAuto → FlyMission` through telemetry only.
#[allow(dead_code)]
SetAutoMode,
PostFlightSync,
}
#[async_trait]
pub trait MissionDriver: Send + Sync {
/// Send `MAV_CMD_COMPONENT_ARM_DISARM(arm=1)` and resolve when the
/// matching `COMMAND_ACK(MAV_RESULT_ACCEPTED)` arrives.
async fn arm(&self) -> Result<(), DriverError>;
/// Send `MAV_CMD_NAV_TAKEOFF` with the configured altitude and
/// resolve when the matching `COMMAND_ACK` arrives. The transition
/// to `MissionState::MissionUploaded` is gated separately on
/// `telemetry.takeoff_complete`.
async fn takeoff(&self, altitude_m: f32) -> Result<(), DriverError>;
/// Run the full mission upload sequence: `MISSION_CLEAR_ALL →
/// MISSION_COUNT → MISSION_ITEM_INT* → MISSION_ACK →
/// MISSION_SET_CURRENT(0)`. The driver MUST return `Err(Rejected)`
/// when the airframe responds with a non-zero `mission_result`.
async fn upload_mission(&self, items: &[MissionWaypoint]) -> Result<(), DriverError>;
/// Send `MAV_CMD_DO_SET_MODE` to AUTO. Used by the fixed-wing
/// variant to transition out of `WaitAuto` programmatically when
/// the operator has not done it externally.
async fn set_auto_mode(&self) -> Result<(), DriverError>;
/// Post-flight push to ground services. Full implementation lands
/// in AZ-652; AZ-648 only requires that the driver expose the
/// entry point so the FSM can advance through `PostFlightSync`.
async fn post_flight_sync(&self) -> Result<(), DriverError>;
}
@@ -0,0 +1,114 @@
//! Fixed-wing transition table:
//! `Disconnected → Connected → HealthOk → BitOk → MissionUploaded →
//! WaitAuto → FlyMission → Land → PostFlightSync → Done`.
//!
//! Operator launches the airframe and sets AUTO externally — the FSM
//! has no `Armed` / `TakeOff` states. Per `architecture.md §5.7`
//! fixed-wing launch is a manual ground-side procedure.
use super::driver::DriverAction;
use super::fsm::{GuardOutcome, Transition};
use super::types::{MissionState, Telemetry};
fn link_up(t: &Telemetry) -> GuardOutcome {
if t.link_up {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
fn health_ok(t: &Telemetry) -> GuardOutcome {
if t.link_up && t.health_ok {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
fn bit_ok(t: &Telemetry) -> GuardOutcome {
if t.link_up && t.health_ok && t.bit_ok {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
// BitOk → MissionUploaded: upload the mission as soon as BIT passes;
// the operator can then switch the airframe to AUTO at any point.
fn upload_precondition(t: &Telemetry) -> GuardOutcome {
if t.link_up && t.bit_ok {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
// MissionUploaded → WaitAuto: pure-advance. WaitAuto is the parking
// state while we wait for the operator's AUTO selection.
fn always_ready(_: &Telemetry) -> GuardOutcome {
GuardOutcome::Ready
}
// WaitAuto → FlyMission: airframe transitioned to AUTO.
fn fly_precondition(t: &Telemetry) -> GuardOutcome {
if t.link_up && t.flight_mode_auto {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
fn land_precondition(t: &Telemetry) -> GuardOutcome {
if t.mission_reached_final {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
fn post_flight_precondition(t: &Telemetry) -> GuardOutcome {
if t.landed_disarmed {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
pub(crate) const TABLE: &[Transition] = &[
Transition::pure(MissionState::Disconnected, MissionState::Connected, link_up),
Transition::pure(MissionState::Connected, MissionState::HealthOk, health_ok),
Transition::pure(MissionState::HealthOk, MissionState::BitOk, bit_ok),
Transition::with_action(
MissionState::BitOk,
MissionState::MissionUploaded,
upload_precondition,
DriverAction::UploadMission,
),
Transition::pure(
MissionState::MissionUploaded,
MissionState::WaitAuto,
always_ready,
),
Transition::pure(
MissionState::WaitAuto,
MissionState::FlyMission,
fly_precondition,
),
Transition::pure(
MissionState::FlyMission,
MissionState::Land,
land_precondition,
),
Transition::pure(
MissionState::Land,
MissionState::PostFlightSync,
post_flight_precondition,
),
Transition::with_action(
MissionState::PostFlightSync,
MissionState::Done,
always_ready,
DriverAction::PostFlightSync,
),
];
+217
View File
@@ -0,0 +1,217 @@
//! Variant-agnostic FSM core. The multirotor and fixed-wing modules
//! supply their own transition tables; this module owns the retry
//! budget, the broadcast event sender, and the "step one transition"
//! protocol shared by both.
use std::collections::HashMap;
use chrono::Utc;
use tokio::sync::broadcast;
use super::driver::{DriverAction, DriverError, MissionDriver};
use super::types::{MissionState, StepOutcome, Telemetry, TransitionEvent, TransitionKey, Variant};
/// Result of a single transition's guard evaluation.
pub(crate) enum GuardOutcome {
/// Precondition met; the FSM should run the action.
Ready,
/// Precondition not met yet; the FSM should wait for the next
/// telemetry tick.
NotYet,
}
/// One row of a per-variant transition table.
pub(crate) struct Transition {
pub from: MissionState,
pub to: MissionState,
/// Inspected each tick; returns `Ready` when the FSM should
/// attempt the action and advance to `to`.
pub guard: fn(&Telemetry) -> GuardOutcome,
/// `Some(action)` for transitions that must issue a driver call
/// (arm, takeoff, mission upload, post-flight sync). `None` for
/// pure telemetry-gated advances (e.g. `Disconnected → Connected`).
pub action: Option<DriverAction>,
}
impl Transition {
pub const fn pure(
from: MissionState,
to: MissionState,
guard: fn(&Telemetry) -> GuardOutcome,
) -> Self {
Self {
from,
to,
guard,
action: None,
}
}
pub const fn with_action(
from: MissionState,
to: MissionState,
guard: fn(&Telemetry) -> GuardOutcome,
action: DriverAction,
) -> Self {
Self {
from,
to,
guard,
action: Some(action),
}
}
}
/// Shared FSM state used by both variants. Each variant owns its
/// transition table (a `&'static [Transition]`).
pub(crate) struct FsmCore {
pub variant: Variant,
pub state: MissionState,
pub retries: HashMap<TransitionKey, u32>,
pub retry_cap: u32,
pub events: broadcast::Sender<TransitionEvent>,
pub paused_reason: Option<String>,
pub mission: Vec<shared::models::mission::MissionWaypoint>,
/// Multirotor takeoff altitude (metres AGL). Ignored for fixed-wing.
pub takeoff_altitude_m: f32,
}
impl FsmCore {
pub fn new(
variant: Variant,
retry_cap: u32,
mission: Vec<shared::models::mission::MissionWaypoint>,
events: broadcast::Sender<TransitionEvent>,
takeoff_altitude_m: f32,
) -> Self {
Self {
variant,
state: MissionState::Disconnected,
retries: HashMap::new(),
retry_cap,
events,
paused_reason: None,
mission,
takeoff_altitude_m,
}
}
pub fn retry_count(&self, key: &TransitionKey) -> u32 {
*self.retries.get(key).unwrap_or(&0)
}
}
/// Attempt to advance the FSM by one transition using the supplied
/// variant-specific transition table.
pub(crate) async fn step_one(
core: &mut FsmCore,
table: &'static [Transition],
telemetry: &Telemetry,
driver: &dyn MissionDriver,
) -> StepOutcome {
// Already-terminal short-circuits.
if core.state == MissionState::Done {
return StepOutcome::AlreadyDone;
}
if core.state == MissionState::Paused {
return StepOutcome::Paused {
reason: core.paused_reason.clone().unwrap_or_default(),
};
}
// Find the transition rooted at the current state. Each state has
// at most one outgoing transition in either variant's table; this
// is the typed-discipline AZ-648 §Outcome calls for.
let Some(t) = table.iter().find(|t| t.from == core.state) else {
return StepOutcome::NoProgress;
};
match (t.guard)(telemetry) {
GuardOutcome::NotYet => return StepOutcome::NoProgress,
GuardOutcome::Ready => {}
}
let key = TransitionKey::new(t.from, t.to);
// Pure-guard transition (no driver action). Advance immediately.
let Some(action) = t.action else {
return advance(core, key);
};
// Driver-action transition. Issue the action; on Ok advance, on
// Err bump the per-transition retry counter and either retry
// (next tick) or pause.
let action_result = run_action(action, core, driver).await;
match action_result {
Ok(()) => {
// Successful action — clear this transition's retry
// counter and advance.
core.retries.remove(&key);
advance(core, key)
}
Err(e) => {
let new_count = core.retries.entry(key).or_insert(0);
*new_count += 1;
let count = *new_count;
tracing::warn!(
from = ?key.from,
to = ?key.to,
action = ?action,
attempt = count,
max = core.retry_cap,
error = %e,
"mission_executor transition retry"
);
if count >= core.retry_cap {
let reason = format!(
"{action:?} cap-exhausted at {from:?}{to:?}: {e}",
from = key.from,
to = key.to,
);
core.state = MissionState::Paused;
core.paused_reason = Some(reason.clone());
let _ = core.events.send(TransitionEvent {
variant: core.variant,
from: key.from,
to: MissionState::Paused,
at: Utc::now(),
retry_count: count,
});
StepOutcome::Paused { reason }
} else {
StepOutcome::Retried {
transition: key,
retry_count: count,
}
}
}
}
}
async fn run_action(
action: DriverAction,
core: &FsmCore,
driver: &dyn MissionDriver,
) -> Result<(), DriverError> {
match action {
DriverAction::Arm => driver.arm().await,
DriverAction::TakeOff => driver.takeoff(core.takeoff_altitude_m).await,
DriverAction::UploadMission => driver.upload_mission(&core.mission).await,
DriverAction::SetAutoMode => driver.set_auto_mode().await,
DriverAction::PostFlightSync => driver.post_flight_sync().await,
}
}
fn advance(core: &mut FsmCore, key: TransitionKey) -> StepOutcome {
let from = core.state;
core.state = key.to;
let retry_count = core.retries.get(&key).copied().unwrap_or(0);
let _ = core.events.send(TransitionEvent {
variant: core.variant,
from,
to: key.to,
at: Utc::now(),
retry_count,
});
StepOutcome::Advanced { from, to: key.to }
}
@@ -0,0 +1,7 @@
//! Internal modules for `mission_executor`. Not part of the public API.
pub mod driver;
pub mod fixed_wing;
pub mod fsm;
pub mod multirotor;
pub mod types;
@@ -0,0 +1,141 @@
//! Multirotor transition table:
//! `Disconnected → Connected → HealthOk → BitOk → Armed → TakeOff →
//! MissionUploaded → FlyMission → Land → PostFlightSync → Done`.
use super::driver::DriverAction;
use super::fsm::{GuardOutcome, Transition};
use super::types::{MissionState, Telemetry};
fn link_up(t: &Telemetry) -> GuardOutcome {
if t.link_up {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
fn health_ok(t: &Telemetry) -> GuardOutcome {
if t.link_up && t.health_ok {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
fn bit_ok(t: &Telemetry) -> GuardOutcome {
if t.link_up && t.health_ok && t.bit_ok {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
// BitOk → Armed: precondition is "BIT passed". Arming itself is a
// driver action; success is reported by COMMAND_ACK in `driver.arm()`.
fn arm_precondition(t: &Telemetry) -> GuardOutcome {
if t.link_up && t.bit_ok {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
// Armed → TakeOff: driver issues `MAV_CMD_NAV_TAKEOFF`; gate is
// telemetry.armed (so we don't issue takeoff before arm ack lands).
fn takeoff_precondition(t: &Telemetry) -> GuardOutcome {
if t.link_up && t.armed {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
// TakeOff → MissionUploaded: wait until takeoff finishes, then upload.
fn upload_precondition(t: &Telemetry) -> GuardOutcome {
if t.link_up && t.takeoff_complete {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
// MissionUploaded → FlyMission: pure-telemetry advance once airframe
// reports it is in the AUTO flight mode (multirotor: ArduPilot sets
// AUTO automatically after `MISSION_SET_CURRENT(0)` + takeoff complete).
fn fly_precondition(t: &Telemetry) -> GuardOutcome {
if t.link_up && t.flight_mode_auto {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
// FlyMission → Land: mission reached its final waypoint.
fn land_precondition(t: &Telemetry) -> GuardOutcome {
if t.mission_reached_final {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
// Land → PostFlightSync: airframe is on the ground and disarmed.
fn post_flight_precondition(t: &Telemetry) -> GuardOutcome {
if t.landed_disarmed {
GuardOutcome::Ready
} else {
GuardOutcome::NotYet
}
}
// PostFlightSync → Done: post-flight sync action returned Ok.
// Guard always returns Ready; the action itself produces the
// retry/fail signal.
fn always_ready(_: &Telemetry) -> GuardOutcome {
GuardOutcome::Ready
}
pub(crate) const TABLE: &[Transition] = &[
Transition::pure(MissionState::Disconnected, MissionState::Connected, link_up),
Transition::pure(MissionState::Connected, MissionState::HealthOk, health_ok),
Transition::pure(MissionState::HealthOk, MissionState::BitOk, bit_ok),
Transition::with_action(
MissionState::BitOk,
MissionState::Armed,
arm_precondition,
DriverAction::Arm,
),
Transition::with_action(
MissionState::Armed,
MissionState::TakeOff,
takeoff_precondition,
DriverAction::TakeOff,
),
Transition::with_action(
MissionState::TakeOff,
MissionState::MissionUploaded,
upload_precondition,
DriverAction::UploadMission,
),
Transition::pure(
MissionState::MissionUploaded,
MissionState::FlyMission,
fly_precondition,
),
Transition::pure(
MissionState::FlyMission,
MissionState::Land,
land_precondition,
),
Transition::pure(
MissionState::Land,
MissionState::PostFlightSync,
post_flight_precondition,
),
Transition::with_action(
MissionState::PostFlightSync,
MissionState::Done,
always_ready,
DriverAction::PostFlightSync,
),
];
@@ -0,0 +1,105 @@
//! Shared types for both variant state machines.
use chrono::{DateTime, Utc};
use serde::{Deserialize, Serialize};
/// Airframe variant. Fixed at startup; no runtime swap (AZ-648 §Constraints).
#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
#[serde(rename_all = "snake_case")]
pub enum Variant {
Multirotor,
FixedWing,
}
/// Union of all states across both variants. Per-variant transition
/// tables limit which states a given variant can reach.
///
/// - Multirotor: `Disconnected → Connected → HealthOk → BitOk → Armed
/// → TakeOff → MissionUploaded → FlyMission → Land → PostFlightSync
/// → Done`.
/// - Fixed-wing: `Disconnected → Connected → HealthOk → BitOk →
/// MissionUploaded → WaitAuto → FlyMission → Land → PostFlightSync
/// → Done`.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)]
#[serde(rename_all = "SCREAMING_SNAKE_CASE")]
pub enum MissionState {
Disconnected,
Connected,
HealthOk,
BitOk,
Armed,
TakeOff,
MissionUploaded,
WaitAuto,
FlyMission,
Land,
PostFlightSync,
Done,
Paused,
}
/// Stable identifier for a single state→state edge. Used to key the
/// per-transition retry counter so a retry budget in one phase
/// doesn't poison another.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, Serialize, Deserialize)]
pub struct TransitionKey {
pub from: MissionState,
pub to: MissionState,
}
impl TransitionKey {
pub const fn new(from: MissionState, to: MissionState) -> Self {
Self { from, to }
}
}
/// Telemetry view fed into each FSM tick. Fields are independent
/// preconditions — different transitions look at different subsets.
/// Updated by `mavlink_layer` consumers in production; injected
/// directly in tests.
#[derive(Debug, Clone, Copy, Default)]
pub struct Telemetry {
pub link_up: bool,
pub health_ok: bool,
pub bit_ok: bool,
pub armed: bool,
pub takeoff_complete: bool,
pub flight_mode_auto: bool,
pub mission_reached_final: bool,
pub landed_disarmed: bool,
}
/// One state→state transition. Recorded for the broadcast event
/// stream and used by `scan_controller` / `telemetry_stream`.
#[derive(Debug, Clone)]
pub struct TransitionEvent {
pub variant: Variant,
pub from: MissionState,
pub to: MissionState,
pub at: DateTime<Utc>,
pub retry_count: u32,
}
/// Outcome of a single FSM step.
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum StepOutcome {
/// Guard not yet satisfied; no transition attempted this tick.
NoProgress,
/// FSM advanced to a new state.
Advanced {
from: MissionState,
to: MissionState,
},
/// The current transition's retry counter incremented (e.g.,
/// mission upload rejected by airframe). Counter value is the
/// post-increment count.
Retried {
transition: TransitionKey,
retry_count: u32,
},
/// Retry budget exhausted for this transition. The FSM is now
/// `MissionState::Paused`; `health()` returns red.
Paused { reason: String },
/// Already-terminal state — no further work.
AlreadyDone,
}
+256 -44
View File
@@ -1,36 +1,94 @@
//! `mission_executor` — multirotor + fixed-wing FSMs, geofence, failsafe.
//!
//! Real implementation lands in:
//! - AZ-648 `mission_executor_state_machine`
//! - AZ-649 `mission_executor_telemetry_forwarding`
//! - AZ-650 `mission_executor_bit_f9`
//! - AZ-651 `mission_executor_lost_link_ladder`
//! - AZ-652 `mission_executor_safety_and_resume`
//! AZ-648 lands the variant-aware state machine, the per-transition
//! retry budget, and the broadcast event stream. Subsequent tasks add:
//! - AZ-649 telemetry forwarding (wires real `Telemetry` from `mavlink_layer`)
//! - AZ-650 BIT F9
//! - AZ-651 lost-link ladder
//! - AZ-652 safety + resume + middle-waypoint insert
//!
//! The FSM core is variant-agnostic; per-variant transition tables in
//! [`internal::multirotor`] and [`internal::fixed_wing`] supply the
//! allowed state graph. Each transition is either:
//! - **Pure** — advances when its `Telemetry` guard returns `Ready`;
//! no driver call is issued.
//! - **Action-bearing** — invokes [`MissionDriver`] (arm, takeoff,
//! mission upload, set-auto, post-flight sync) and only advances on
//! `Ok(())`. On `Err` the per-transition retry counter increments;
//! on cap exhaustion the FSM moves to [`MissionState::Paused`] and
//! health flips to red.
use std::sync::Arc;
use std::time::Duration;
use serde::{Deserialize, Serialize};
use tokio::sync::{broadcast, watch, Mutex};
use tokio::task::JoinHandle;
use tokio::time::Instant;
use shared::error::{AutopilotError, Result};
use shared::health::ComponentHealth;
use shared::models::mission::{Coordinate, MissionItem};
use shared::models::mission::{Coordinate, MissionItem, MissionWaypoint};
mod internal;
pub use internal::driver::{DriverError, MissionDriver};
pub use internal::types::{
MissionState, StepOutcome, Telemetry, TransitionEvent, TransitionKey, Variant,
};
use internal::fsm::{step_one, FsmCore};
const NAME: &str = "mission_executor";
#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
#[serde(rename_all = "SCREAMING_SNAKE_CASE")]
pub enum ExecutorState {
Disconnected,
PreFlight,
Taxi,
Climb,
Cruise,
MiddleWaypointInsert,
TargetFollow,
Rtl,
Land,
WaitAuto,
Aborted,
/// Default per-transition retry budget per AZ-648 §Non-Functional Requirements.
pub const DEFAULT_RETRY_CAP: u32 = 3;
/// Default tick interval. ≤10 ms p99 budget per AZ-648; we tick at 50 Hz
/// so each tick has ample headroom for one driver call.
pub const DEFAULT_TICK: Duration = Duration::from_millis(20);
/// FSM construction parameters.
#[derive(Debug, Clone)]
pub struct MissionExecutorConfig {
pub variant: Variant,
/// Multirotor only. Ignored for fixed-wing.
pub takeoff_altitude_m: f32,
/// Default = [`DEFAULT_RETRY_CAP`].
pub retry_cap: u32,
/// Default = [`DEFAULT_TICK`].
pub tick_interval: Duration,
/// Broadcast channel capacity for [`TransitionEvent`]. Consumers
/// that lag past this fall behind and lose events; transitions
/// themselves still happen.
pub event_channel_capacity: usize,
}
impl MissionExecutorConfig {
pub fn multirotor(takeoff_altitude_m: f32) -> Self {
Self {
variant: Variant::Multirotor,
takeoff_altitude_m,
retry_cap: DEFAULT_RETRY_CAP,
tick_interval: DEFAULT_TICK,
event_channel_capacity: 64,
}
}
pub fn fixed_wing() -> Self {
Self {
variant: Variant::FixedWing,
takeoff_altitude_m: 0.0,
retry_cap: DEFAULT_RETRY_CAP,
tick_interval: DEFAULT_TICK,
event_channel_capacity: 64,
}
}
}
// Legacy enums retained for AZ-651 / AZ-652 to consume. Not part of the
// AZ-648 surface but still publicly exported to keep the public crate
// API stable.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize)]
#[serde(rename_all = "snake_case")]
pub enum FailsafeKind {
@@ -43,34 +101,140 @@ pub enum FailsafeKind {
GeofenceExclusion,
}
pub struct MissionExecutor;
/// Top-level executor. Construct, then call [`MissionExecutor::run`]
/// to spawn the FSM task. The returned [`MissionExecutorHandle`] is
/// the read-side: state, health, transition event subscription.
pub struct MissionExecutor {
config: MissionExecutorConfig,
}
impl MissionExecutor {
pub fn new() -> Self {
Self
pub fn new(config: MissionExecutorConfig) -> Self {
Self { config }
}
pub fn handle(&self) -> MissionExecutorHandle {
MissionExecutorHandle
/// Spawn the FSM driver. Returns a handle to read state and a join
/// handle for the background task.
///
/// `telemetry_rx` is a `watch::Receiver` so the producer (the
/// `mavlink_layer` telemetry forwarder per AZ-649) can publish the
/// latest snapshot without back-pressure. Each tick reads the
/// current value; missed intermediate updates are intentionally
/// dropped (the guards are level-triggered).
pub fn run<D>(
&self,
driver: Arc<D>,
mission: Vec<MissionWaypoint>,
telemetry_rx: watch::Receiver<Telemetry>,
) -> (MissionExecutorHandle, JoinHandle<()>)
where
D: MissionDriver + 'static,
{
let (events_tx, _events_rx) = broadcast::channel(self.config.event_channel_capacity.max(1));
let core = FsmCore::new(
self.config.variant,
self.config.retry_cap,
mission,
events_tx.clone(),
self.config.takeoff_altitude_m,
);
let core = Arc::new(Mutex::new(core));
let table: &'static [internal::fsm::Transition] = match self.config.variant {
Variant::Multirotor => internal::multirotor::TABLE,
Variant::FixedWing => internal::fixed_wing::TABLE,
};
let tick = self.config.tick_interval;
let core_for_task = core.clone();
let driver_for_task: Arc<dyn MissionDriver> = driver;
let handle = MissionExecutorHandle {
core: core.clone(),
events_tx: events_tx.clone(),
};
let join = tokio::spawn(async move {
run_loop(core_for_task, table, driver_for_task, telemetry_rx, tick).await;
});
(handle, join)
}
}
impl Default for MissionExecutor {
fn default() -> Self {
Self::new()
async fn run_loop(
core: Arc<Mutex<FsmCore>>,
table: &'static [internal::fsm::Transition],
driver: Arc<dyn MissionDriver>,
mut telemetry_rx: watch::Receiver<Telemetry>,
tick: Duration,
) {
let mut ticker = tokio::time::interval_at(Instant::now() + tick, tick);
ticker.set_missed_tick_behavior(tokio::time::MissedTickBehavior::Skip);
loop {
ticker.tick().await;
let telemetry = *telemetry_rx.borrow_and_update();
let mut guard = core.lock().await;
let outcome = step_one(&mut guard, table, &telemetry, driver.as_ref()).await;
let terminal = matches!(
outcome,
StepOutcome::AlreadyDone | StepOutcome::Paused { .. }
);
drop(guard);
if terminal {
return;
}
}
}
#[derive(Clone, Copy)]
pub struct MissionExecutorHandle;
/// Read-side handle. Clone-safe.
#[derive(Clone)]
pub struct MissionExecutorHandle {
core: Arc<Mutex<FsmCore>>,
events_tx: broadcast::Sender<TransitionEvent>,
}
impl MissionExecutorHandle {
pub async fn start(&self, _mission: Vec<MissionItem>) -> Result<()> {
Err(AutopilotError::NotImplemented(
"mission_executor::start (AZ-648)",
))
/// Current FSM state. Cheap (single mutex lock).
pub async fn state(&self) -> MissionState {
self.core.lock().await.state
}
/// Subscribe to the broadcast stream of [`TransitionEvent`]s.
/// Each new subscriber starts from the next event published; past
/// events are not replayed.
pub fn subscribe(&self) -> broadcast::Receiver<TransitionEvent> {
self.events_tx.subscribe()
}
/// Post-increment retry counter for the given transition.
pub async fn retry_count(&self, key: TransitionKey) -> u32 {
self.core.lock().await.retry_count(&key)
}
/// Reason the FSM paused, if it is paused.
pub async fn paused_reason(&self) -> Option<String> {
self.core.lock().await.paused_reason.clone()
}
/// Aggregated health: red when paused, green when `Done`,
/// yellow otherwise.
pub async fn health(&self) -> ComponentHealth {
let guard = self.core.lock().await;
match guard.state {
MissionState::Paused => {
let reason = guard
.paused_reason
.clone()
.unwrap_or_else(|| "paused".to_string());
ComponentHealth::red(NAME, reason)
}
MissionState::Done => ComponentHealth::green(NAME).with_detail("mission complete"),
other => ComponentHealth::yellow(NAME, format!("state={other:?}")),
}
}
/// Single-shot RPC-style endpoints kept on the handle for the
/// follow-up tasks (AZ-651/AZ-652). Today they return `NotImplemented`.
pub async fn insert_middle_waypoint(&self, _at: Coordinate) -> Result<()> {
Err(AutopilotError::NotImplemented(
"mission_executor::insert_middle_waypoint (AZ-652)",
@@ -83,23 +247,71 @@ impl MissionExecutorHandle {
))
}
pub fn state(&self) -> ExecutorState {
ExecutorState::Disconnected
/// Pre-AZ-648 helper kept for callers that only need to validate a
/// mission shape. The proper start path is [`MissionExecutor::run`].
pub async fn start(&self, _mission: Vec<MissionItem>) -> Result<()> {
Err(AutopilotError::NotImplemented(
"mission_executor::start: use MissionExecutor::run (AZ-648)",
))
}
}
pub fn health(&self) -> ComponentHealth {
ComponentHealth::disabled(NAME)
trait HealthDetail {
fn with_detail(self, detail: impl Into<String>) -> Self;
}
impl HealthDetail for ComponentHealth {
fn with_detail(mut self, detail: impl Into<String>) -> Self {
self.detail = Some(detail.into());
self
}
}
#[cfg(test)]
mod tests {
use super::*;
use async_trait::async_trait;
#[test]
fn it_compiles() {
let h = MissionExecutor::new().handle();
assert_eq!(h.state(), ExecutorState::Disconnected);
assert_eq!(h.health().level, shared::health::HealthLevel::Disabled);
struct NeverCalledDriver;
#[async_trait]
impl MissionDriver for NeverCalledDriver {
async fn arm(&self) -> std::result::Result<(), DriverError> {
panic!("arm called");
}
async fn takeoff(&self, _altitude_m: f32) -> std::result::Result<(), DriverError> {
panic!("takeoff called");
}
async fn upload_mission(
&self,
_items: &[MissionWaypoint],
) -> std::result::Result<(), DriverError> {
panic!("upload_mission called");
}
async fn set_auto_mode(&self) -> std::result::Result<(), DriverError> {
panic!("set_auto_mode called");
}
async fn post_flight_sync(&self) -> std::result::Result<(), DriverError> {
panic!("post_flight_sync called");
}
}
#[tokio::test]
async fn handle_starts_in_disconnected_with_yellow_health() {
// Arrange
let exec = MissionExecutor::new(MissionExecutorConfig::multirotor(10.0));
let (_tx, rx) = watch::channel(Telemetry::default());
let driver = Arc::new(NeverCalledDriver);
// Act
let (handle, join) = exec.run(driver, vec![], rx);
// Assert
assert_eq!(handle.state().await, MissionState::Disconnected);
let health = handle.health().await;
assert_eq!(health.level, shared::health::HealthLevel::Yellow);
// Cleanup
join.abort();
}
}