[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
+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();
}
}