[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,
}