mirror of
https://github.com/azaion/autopilot.git
synced 2026-06-21 22:01:10 +00:00
[AZ-666] [AZ-673] [AZ-648] ignored set + UDS VLM + mission FSM batch 5
ci/woodpecker/push/build-arm Pipeline failed
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:
@@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user