mirror of
https://github.com/azaion/autopilot.git
synced 2026-06-21 18:21: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:
@@ -15,3 +15,6 @@ mapobjects_store = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
serde = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
async-trait = { workspace = true }
|
||||
chrono = { workspace = true }
|
||||
|
||||
@@ -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,
|
||||
),
|
||||
];
|
||||
@@ -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,
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,448 @@
|
||||
//! AZ-648 acceptance criteria.
|
||||
//!
|
||||
//! AC-1 / AC-2 — happy-path multirotor / fixed-wing flow with a fake
|
||||
//! driver. The driver stands in for the SITL conformance target; the
|
||||
//! state graph and event publication are what the AC asserts.
|
||||
//!
|
||||
//! AC-3 — bounded retry on mission-upload rejection: first attempt
|
||||
//! rejected, second succeeds, FSM proceeds.
|
||||
//!
|
||||
//! AC-4 — cap exhaustion: all 3 default attempts rejected → FSM pauses,
|
||||
//! health → red, transition event published, no transition past
|
||||
//! `MissionUploaded`.
|
||||
|
||||
use std::sync::atomic::{AtomicU32, Ordering};
|
||||
use std::sync::Arc;
|
||||
use std::time::Duration;
|
||||
|
||||
use async_trait::async_trait;
|
||||
use mission_executor::{
|
||||
DriverError, MissionDriver, MissionExecutor, MissionExecutorConfig, MissionState, StepOutcome,
|
||||
Telemetry, TransitionKey, Variant, DEFAULT_RETRY_CAP,
|
||||
};
|
||||
use shared::health::HealthLevel;
|
||||
use shared::models::mission::{MavCommand, MavFrame, MissionWaypoint};
|
||||
use tokio::sync::watch;
|
||||
use tokio::time::timeout;
|
||||
|
||||
/// Configurable in-memory driver. Counts every action and can be
|
||||
/// scripted to reject the next N upload calls.
|
||||
struct ScriptedDriver {
|
||||
arm_calls: AtomicU32,
|
||||
takeoff_calls: AtomicU32,
|
||||
upload_calls: AtomicU32,
|
||||
set_auto_calls: AtomicU32,
|
||||
post_flight_calls: AtomicU32,
|
||||
reject_first_n_uploads: AtomicU32,
|
||||
}
|
||||
|
||||
impl ScriptedDriver {
|
||||
fn new() -> Arc<Self> {
|
||||
Arc::new(Self {
|
||||
arm_calls: AtomicU32::new(0),
|
||||
takeoff_calls: AtomicU32::new(0),
|
||||
upload_calls: AtomicU32::new(0),
|
||||
set_auto_calls: AtomicU32::new(0),
|
||||
post_flight_calls: AtomicU32::new(0),
|
||||
reject_first_n_uploads: AtomicU32::new(0),
|
||||
})
|
||||
}
|
||||
|
||||
fn reject_next_uploads(&self, n: u32) {
|
||||
self.reject_first_n_uploads.store(n, Ordering::SeqCst);
|
||||
}
|
||||
}
|
||||
|
||||
#[async_trait]
|
||||
impl MissionDriver for ScriptedDriver {
|
||||
async fn arm(&self) -> Result<(), DriverError> {
|
||||
self.arm_calls.fetch_add(1, Ordering::SeqCst);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
async fn takeoff(&self, _altitude_m: f32) -> Result<(), DriverError> {
|
||||
self.takeoff_calls.fetch_add(1, Ordering::SeqCst);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
async fn upload_mission(&self, _items: &[MissionWaypoint]) -> Result<(), DriverError> {
|
||||
self.upload_calls.fetch_add(1, Ordering::SeqCst);
|
||||
loop {
|
||||
let remaining = self.reject_first_n_uploads.load(Ordering::SeqCst);
|
||||
if remaining == 0 {
|
||||
return Ok(());
|
||||
}
|
||||
if self
|
||||
.reject_first_n_uploads
|
||||
.compare_exchange(remaining, remaining - 1, Ordering::SeqCst, Ordering::SeqCst)
|
||||
.is_ok()
|
||||
{
|
||||
return Err(DriverError::Rejected("simulated".into()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
async fn set_auto_mode(&self) -> Result<(), DriverError> {
|
||||
self.set_auto_calls.fetch_add(1, Ordering::SeqCst);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
async fn post_flight_sync(&self) -> Result<(), DriverError> {
|
||||
self.post_flight_calls.fetch_add(1, Ordering::SeqCst);
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
/// Drive `telemetry_rx` forward through a script while polling the
|
||||
/// executor until `target` is reached. The script entries are applied
|
||||
/// in order — each one waits up to `step_timeout` for the FSM to
|
||||
/// advance past `prev`, then publishes the next telemetry snapshot.
|
||||
async fn await_state(
|
||||
handle: &mission_executor::MissionExecutorHandle,
|
||||
target: MissionState,
|
||||
overall: Duration,
|
||||
) {
|
||||
let res = timeout(overall, async {
|
||||
loop {
|
||||
if handle.state().await == target {
|
||||
return;
|
||||
}
|
||||
tokio::time::sleep(Duration::from_millis(5)).await;
|
||||
}
|
||||
})
|
||||
.await;
|
||||
if res.is_err() {
|
||||
let actual = handle.state().await;
|
||||
panic!("FSM did not reach {target:?}; stuck at {actual:?}");
|
||||
}
|
||||
}
|
||||
|
||||
fn one_waypoint() -> Vec<MissionWaypoint> {
|
||||
vec![MissionWaypoint {
|
||||
seq: 0,
|
||||
frame: MavFrame::MavFrameGlobalRelativeAlt,
|
||||
command: MavCommand::MavCmdNavWaypoint,
|
||||
current: true,
|
||||
auto_continue: true,
|
||||
param_1: 0.0,
|
||||
param_2: 0.0,
|
||||
param_3: 0.0,
|
||||
param_4: 0.0,
|
||||
lat_deg_e7: 0,
|
||||
lon_deg_e7: 0,
|
||||
alt_m: 50.0,
|
||||
}]
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn ac1_multirotor_happy_path_reaches_done() {
|
||||
// Arrange
|
||||
let driver = ScriptedDriver::new();
|
||||
let exec = MissionExecutor::new(MissionExecutorConfig {
|
||||
tick_interval: Duration::from_millis(5),
|
||||
..MissionExecutorConfig::multirotor(10.0)
|
||||
});
|
||||
let (tx, rx) = watch::channel(Telemetry::default());
|
||||
let (handle, join) = exec.run(driver.clone(), one_waypoint(), rx);
|
||||
let mut events = handle.subscribe();
|
||||
|
||||
// Act / Assert — step the telemetry script.
|
||||
tx.send(Telemetry {
|
||||
link_up: true,
|
||||
..Telemetry::default()
|
||||
})
|
||||
.unwrap();
|
||||
await_state(&handle, MissionState::Connected, Duration::from_secs(1)).await;
|
||||
|
||||
tx.send(Telemetry {
|
||||
link_up: true,
|
||||
health_ok: true,
|
||||
..Telemetry::default()
|
||||
})
|
||||
.unwrap();
|
||||
await_state(&handle, MissionState::HealthOk, Duration::from_secs(1)).await;
|
||||
|
||||
tx.send(Telemetry {
|
||||
link_up: true,
|
||||
health_ok: true,
|
||||
bit_ok: true,
|
||||
..Telemetry::default()
|
||||
})
|
||||
.unwrap();
|
||||
await_state(&handle, MissionState::BitOk, Duration::from_secs(1)).await;
|
||||
await_state(&handle, MissionState::Armed, Duration::from_secs(1)).await;
|
||||
|
||||
tx.send(Telemetry {
|
||||
link_up: true,
|
||||
health_ok: true,
|
||||
bit_ok: true,
|
||||
armed: true,
|
||||
..Telemetry::default()
|
||||
})
|
||||
.unwrap();
|
||||
await_state(&handle, MissionState::TakeOff, Duration::from_secs(1)).await;
|
||||
|
||||
tx.send(Telemetry {
|
||||
link_up: true,
|
||||
health_ok: true,
|
||||
bit_ok: true,
|
||||
armed: true,
|
||||
takeoff_complete: true,
|
||||
..Telemetry::default()
|
||||
})
|
||||
.unwrap();
|
||||
await_state(
|
||||
&handle,
|
||||
MissionState::MissionUploaded,
|
||||
Duration::from_secs(1),
|
||||
)
|
||||
.await;
|
||||
|
||||
tx.send(Telemetry {
|
||||
link_up: true,
|
||||
health_ok: true,
|
||||
bit_ok: true,
|
||||
armed: true,
|
||||
takeoff_complete: true,
|
||||
flight_mode_auto: true,
|
||||
..Telemetry::default()
|
||||
})
|
||||
.unwrap();
|
||||
await_state(&handle, MissionState::FlyMission, Duration::from_secs(1)).await;
|
||||
|
||||
tx.send(Telemetry {
|
||||
link_up: true,
|
||||
health_ok: true,
|
||||
bit_ok: true,
|
||||
armed: true,
|
||||
takeoff_complete: true,
|
||||
flight_mode_auto: true,
|
||||
mission_reached_final: true,
|
||||
..Telemetry::default()
|
||||
})
|
||||
.unwrap();
|
||||
await_state(&handle, MissionState::Land, Duration::from_secs(1)).await;
|
||||
|
||||
tx.send(Telemetry {
|
||||
link_up: true,
|
||||
health_ok: true,
|
||||
bit_ok: true,
|
||||
armed: false,
|
||||
takeoff_complete: true,
|
||||
flight_mode_auto: true,
|
||||
mission_reached_final: true,
|
||||
landed_disarmed: true,
|
||||
})
|
||||
.unwrap();
|
||||
await_state(
|
||||
&handle,
|
||||
MissionState::PostFlightSync,
|
||||
Duration::from_secs(1),
|
||||
)
|
||||
.await;
|
||||
await_state(&handle, MissionState::Done, Duration::from_secs(1)).await;
|
||||
|
||||
// Assert — health is green at Done, driver saw exactly one of each action.
|
||||
let health = handle.health().await;
|
||||
assert_eq!(health.level, HealthLevel::Green);
|
||||
assert_eq!(driver.arm_calls.load(Ordering::SeqCst), 1);
|
||||
assert_eq!(driver.takeoff_calls.load(Ordering::SeqCst), 1);
|
||||
assert_eq!(driver.upload_calls.load(Ordering::SeqCst), 1);
|
||||
assert_eq!(driver.post_flight_calls.load(Ordering::SeqCst), 1);
|
||||
// No fixed-wing action on a multirotor flow.
|
||||
assert_eq!(driver.set_auto_calls.load(Ordering::SeqCst), 0);
|
||||
|
||||
// Drain the event stream — count distinct transitions; we expect
|
||||
// every state above to appear in order.
|
||||
let mut observed = Vec::new();
|
||||
while let Ok(evt) = events.try_recv() {
|
||||
observed.push((evt.from, evt.to));
|
||||
}
|
||||
assert!(observed.contains(&(MissionState::Disconnected, MissionState::Connected)));
|
||||
assert!(observed.contains(&(MissionState::PostFlightSync, MissionState::Done)));
|
||||
|
||||
let _ = join.await;
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn ac2_fixed_wing_happy_path_reaches_done() {
|
||||
// Arrange — fixed-wing skips Armed/TakeOff. The operator sets AUTO
|
||||
// externally; we model that by flipping `flight_mode_auto` while
|
||||
// the FSM is parked in WaitAuto.
|
||||
let driver = ScriptedDriver::new();
|
||||
let exec = MissionExecutor::new(MissionExecutorConfig {
|
||||
tick_interval: Duration::from_millis(5),
|
||||
..MissionExecutorConfig::fixed_wing()
|
||||
});
|
||||
let (tx, rx) = watch::channel(Telemetry::default());
|
||||
let (handle, join) = exec.run(driver.clone(), one_waypoint(), rx);
|
||||
|
||||
// Act / Assert
|
||||
tx.send(Telemetry {
|
||||
link_up: true,
|
||||
health_ok: true,
|
||||
bit_ok: true,
|
||||
..Telemetry::default()
|
||||
})
|
||||
.unwrap();
|
||||
await_state(&handle, MissionState::WaitAuto, Duration::from_secs(2)).await;
|
||||
assert_eq!(driver.arm_calls.load(Ordering::SeqCst), 0);
|
||||
assert_eq!(driver.takeoff_calls.load(Ordering::SeqCst), 0);
|
||||
assert_eq!(driver.upload_calls.load(Ordering::SeqCst), 1);
|
||||
|
||||
tx.send(Telemetry {
|
||||
link_up: true,
|
||||
health_ok: true,
|
||||
bit_ok: true,
|
||||
flight_mode_auto: true,
|
||||
..Telemetry::default()
|
||||
})
|
||||
.unwrap();
|
||||
await_state(&handle, MissionState::FlyMission, Duration::from_secs(1)).await;
|
||||
|
||||
tx.send(Telemetry {
|
||||
link_up: true,
|
||||
health_ok: true,
|
||||
bit_ok: true,
|
||||
flight_mode_auto: true,
|
||||
mission_reached_final: true,
|
||||
..Telemetry::default()
|
||||
})
|
||||
.unwrap();
|
||||
await_state(&handle, MissionState::Land, Duration::from_secs(1)).await;
|
||||
|
||||
tx.send(Telemetry {
|
||||
link_up: true,
|
||||
health_ok: true,
|
||||
bit_ok: true,
|
||||
flight_mode_auto: true,
|
||||
mission_reached_final: true,
|
||||
landed_disarmed: true,
|
||||
..Telemetry::default()
|
||||
})
|
||||
.unwrap();
|
||||
await_state(&handle, MissionState::Done, Duration::from_secs(2)).await;
|
||||
|
||||
assert_eq!(handle.health().await.level, HealthLevel::Green);
|
||||
let _ = join.await;
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn ac3_bounded_retry_then_success() {
|
||||
// Arrange — reject the first upload attempt, accept the second.
|
||||
let driver = ScriptedDriver::new();
|
||||
driver.reject_next_uploads(1);
|
||||
let exec = MissionExecutor::new(MissionExecutorConfig {
|
||||
tick_interval: Duration::from_millis(5),
|
||||
..MissionExecutorConfig::fixed_wing()
|
||||
});
|
||||
let (tx, rx) = watch::channel(Telemetry {
|
||||
link_up: true,
|
||||
health_ok: true,
|
||||
bit_ok: true,
|
||||
..Telemetry::default()
|
||||
});
|
||||
let (handle, join) = exec.run(driver.clone(), one_waypoint(), rx);
|
||||
|
||||
// Act
|
||||
await_state(
|
||||
&handle,
|
||||
MissionState::MissionUploaded,
|
||||
Duration::from_secs(2),
|
||||
)
|
||||
.await;
|
||||
|
||||
// Assert — driver was called twice (one rejected + one accepted),
|
||||
// retry counter for that transition is 1, FSM proceeded.
|
||||
assert_eq!(driver.upload_calls.load(Ordering::SeqCst), 2);
|
||||
let retry = handle
|
||||
.retry_count(TransitionKey::new(
|
||||
MissionState::BitOk,
|
||||
MissionState::MissionUploaded,
|
||||
))
|
||||
.await;
|
||||
// Successful advance clears the retry counter (per FSM design —
|
||||
// a fresh transition starts with a clean budget). The proof that
|
||||
// a retry happened is the double upload_calls.
|
||||
assert_eq!(retry, 0);
|
||||
assert!(matches!(
|
||||
handle.state().await,
|
||||
MissionState::WaitAuto | MissionState::MissionUploaded
|
||||
));
|
||||
|
||||
// Cleanup — drive to Done so the task exits cleanly.
|
||||
tx.send(Telemetry {
|
||||
link_up: true,
|
||||
health_ok: true,
|
||||
bit_ok: true,
|
||||
flight_mode_auto: true,
|
||||
mission_reached_final: true,
|
||||
landed_disarmed: true,
|
||||
..Telemetry::default()
|
||||
})
|
||||
.unwrap();
|
||||
await_state(&handle, MissionState::Done, Duration::from_secs(2)).await;
|
||||
let _ = join.await;
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
async fn ac4_cap_exhaustion_pauses_and_flips_health_red() {
|
||||
// Arrange — reject every upload attempt. With the default cap of 3
|
||||
// the FSM should pause on the 3rd failure.
|
||||
let driver = ScriptedDriver::new();
|
||||
driver.reject_next_uploads(u32::MAX);
|
||||
let exec = MissionExecutor::new(MissionExecutorConfig {
|
||||
tick_interval: Duration::from_millis(5),
|
||||
..MissionExecutorConfig::fixed_wing()
|
||||
});
|
||||
let (_tx, rx) = watch::channel(Telemetry {
|
||||
link_up: true,
|
||||
health_ok: true,
|
||||
bit_ok: true,
|
||||
..Telemetry::default()
|
||||
});
|
||||
let (handle, join) = exec.run(driver.clone(), one_waypoint(), rx);
|
||||
let mut events = handle.subscribe();
|
||||
|
||||
// Act
|
||||
await_state(&handle, MissionState::Paused, Duration::from_secs(2)).await;
|
||||
|
||||
// Assert
|
||||
assert_eq!(
|
||||
driver.upload_calls.load(Ordering::SeqCst),
|
||||
DEFAULT_RETRY_CAP,
|
||||
"driver should have been called exactly cap times"
|
||||
);
|
||||
let health = handle.health().await;
|
||||
assert_eq!(health.level, HealthLevel::Red);
|
||||
let reason = handle.paused_reason().await.expect("paused reason");
|
||||
assert!(
|
||||
reason.contains("UploadMission") || reason.contains("cap-exhausted"),
|
||||
"reason should mention the failed action: got {reason}"
|
||||
);
|
||||
|
||||
// A `→ Paused` event must have been published.
|
||||
let mut saw_pause_event = false;
|
||||
while let Ok(evt) = events.try_recv() {
|
||||
if evt.to == MissionState::Paused {
|
||||
saw_pause_event = true;
|
||||
assert_eq!(evt.variant, Variant::FixedWing);
|
||||
break;
|
||||
}
|
||||
}
|
||||
assert!(
|
||||
saw_pause_event,
|
||||
"expected a transition event with to=Paused"
|
||||
);
|
||||
|
||||
// FSM does not advance past MissionUploaded — we never reached it.
|
||||
// Task exits because the state is terminal.
|
||||
let final_state = handle.state().await;
|
||||
assert_eq!(final_state, MissionState::Paused);
|
||||
let final_outcome = StepOutcome::Paused {
|
||||
reason: reason.clone(),
|
||||
};
|
||||
assert!(matches!(final_outcome, StepOutcome::Paused { .. }));
|
||||
|
||||
let _ = join.await;
|
||||
}
|
||||
Reference in New Issue
Block a user