[AZ-652] mission_executor safety + resume + middle-waypoint (batch 9)

Geofence (INCLUSION+EXCLUSION, ≤500 ms detect→RTL), battery
thresholds (RTL@25%/land@15% + signed override), middle-waypoint
re-upload (CLEAR_ALL→upload→SET_CURRENT(0)), and post-flight
mapobjects push trigger. Adds production MAVLink command issuers
for both geofence and battery failsafe families.

Implements 6 ACs with 12 integration tests + module unit tests;
full workspace test suite green. See batch_09_cycle1_report.md
for AC coverage and known limitations.

Co-authored-by: Cursor <cursoragent@cursor.com>

crates/mission_executor/src/internal/battery_thresholds.rs

@@ -0,0 +1,561 @@
+//! AZ-652 — battery / fuel threshold enforcement.
+//!
+//! Two thresholds defined by the task spec:
+//!
+//! - `rtl_threshold_pct` (default 25 %) — battery below this returns
+//!   the UAV to launch via `MAV_CMD_NAV_RETURN_TO_LAUNCH`. A signed
+//!   operator override can suppress this until a configurable
+//!   deadline (AC-4).
+//! - `hard_floor_pct` (default 15 %) — battery below this lands the
+//!   UAV at the safest reachable point via `MAV_CMD_NAV_LAND`.
+//!   **Hard floor cannot be overridden** — even a signed override
+//!   only suppresses RTL, never the land-now safety floor.
+//!
+//! The monitor is **pure logic**: `tick(sys_status, now)` is
+//! deterministic. The driver in [`BatteryDriver`] subscribes to the
+//! `UavSysStatus` watch channel that `mission_executor`'s telemetry
+//! forwarder publishes (AZ-649), runs the monitor on a 100 ms tick,
+//! and dispatches the executor failsafe + the MAVLink command via the
+//! supplied [`BatteryCommandIssuer`].
+//!
+//! ## Audit log
+//!
+//! The task spec excludes the persistent audit log layer
+//! (`shared::audit`, to land separately). We surface override
+//! application via a `tracing::warn!` entry and a
+//! [`BatteryEvent::OverrideApplied`] broadcast event so downstream
+//! consumers can record it.
+
+use std::sync::Arc;
+use std::time::Duration;
+
+use async_trait::async_trait;
+use mavlink_layer::{CommandLong, MavlinkHandle, SendCommandError};
+use tokio::sync::{broadcast, watch};
+use tokio::task::JoinHandle;
+use tokio::time::Instant;
+
+use shared::error::AutopilotError;
+use shared::models::telemetry::UavSysStatus;
+
+use crate::internal::lost_link::MAV_CMD_NAV_RETURN_TO_LAUNCH;
+use crate::FailsafeKind;
+use crate::MissionExecutorHandle;
+
+/// MAVLink `MAV_CMD_NAV_LAND` command id (per the MAVLink Common spec).
+pub const MAV_CMD_NAV_LAND: u16 = 21;
+
+/// Threshold configuration. Defaults follow the task spec.
+#[derive(Debug, Clone, Copy)]
+pub struct BatteryConfig {
+    pub rtl_threshold_pct: u8,
+    pub hard_floor_pct: u8,
+}
+
+impl Default for BatteryConfig {
+    fn default() -> Self {
+        Self {
+            rtl_threshold_pct: 25,
+            hard_floor_pct: 15,
+        }
+    }
+}
+
+/// Signed operator override of the RTL threshold. The signature is
+/// pre-validated by `operator_bridge` (AZ-678/AZ-681 lane); by the
+/// time the override reaches this monitor, only the deadline matters.
+///
+/// `operator_id` and `rationale` are carried for the audit log and
+/// observability; they do not affect the decision logic.
+#[derive(Debug, Clone)]
+pub struct BatteryOverride {
+    pub until: Instant,
+    pub operator_id: String,
+    pub rationale: String,
+}
+
+/// Outcome of a single tick.
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum BatteryAction {
+    /// No action this tick.
+    None,
+    /// Battery ≤ `rtl_threshold_pct`. Issue `MAV_CMD_NAV_RETURN_TO_LAUNCH`
+    /// and trigger executor failsafe `BatteryRtl`.
+    IssueRtl,
+    /// Battery ≤ `hard_floor_pct`. Issue `MAV_CMD_NAV_LAND` and trigger
+    /// executor failsafe `BatteryHardFloor`. Hard floor is honoured
+    /// regardless of any active override.
+    IssueLandNow,
+    /// RTL would have fired but was suppressed by an active operator
+    /// override.
+    SuppressedByOverride,
+}
+
+impl BatteryAction {
+    pub fn failsafe_kind(self) -> Option<FailsafeKind> {
+        match self {
+            BatteryAction::None | BatteryAction::SuppressedByOverride => None,
+            BatteryAction::IssueRtl => Some(FailsafeKind::BatteryRtl),
+            BatteryAction::IssueLandNow => Some(FailsafeKind::BatteryHardFloor),
+        }
+    }
+}
+
+/// Pure battery monitor. Owns the threshold configuration, the active
+/// override (if any), and the "we already fired RTL once" latch so a
+/// fluctuating reading does not produce a flood of duplicate triggers.
+#[derive(Debug)]
+pub struct BatteryMonitor {
+    config: BatteryConfig,
+    override_until: Option<BatteryOverride>,
+    rtl_latched: bool,
+    land_latched: bool,
+}
+
+impl BatteryMonitor {
+    pub fn new(config: BatteryConfig) -> Self {
+        Self {
+            config,
+            override_until: None,
+            rtl_latched: false,
+            land_latched: false,
+        }
+    }
+
+    pub fn config(&self) -> BatteryConfig {
+        self.config
+    }
+
+    pub fn override_active(&self, now: Instant) -> bool {
+        self.override_until
+            .as_ref()
+            .map(|o| o.until > now)
+            .unwrap_or(false)
+    }
+
+    /// Apply a signed operator override. Replaces any prior override
+    /// in flight. Idempotent. The caller (operator_bridge) is
+    /// responsible for signature validation BEFORE invoking this.
+    pub fn apply_override(&mut self, override_: BatteryOverride) {
+        tracing::warn!(
+            until_unix_ns = override_.until.elapsed().as_nanos() as i128,
+            operator_id = %override_.operator_id,
+            rationale = %override_.rationale,
+            "battery RTL override applied"
+        );
+        self.override_until = Some(override_);
+    }
+
+    /// Reset both latches. Used after the FSM acknowledges the
+    /// failsafe so subsequent improvements in battery readings can
+    /// re-arm the monitor (e.g. battery swap on the ground).
+    pub fn reset_latches(&mut self) {
+        self.rtl_latched = false;
+        self.land_latched = false;
+    }
+
+    /// Single-shot decision. Hard floor is checked first (more
+    /// severe + not overridable). `now` is consulted only for the
+    /// override deadline.
+    pub fn tick(&mut self, sys_status: &UavSysStatus, now: Instant) -> BatteryAction {
+        // `battery_remaining: i8` is the standard MAVLink encoding for
+        // percent — `-1` means "unknown / not reporting". Treat unknown
+        // as no-action; the BIT pre-flight gate already requires a
+        // valid reading at startup.
+        let remaining = sys_status.battery_remaining;
+        if remaining < 0 {
+            return BatteryAction::None;
+        }
+        let pct = remaining as u8;
+
+        if pct <= self.config.hard_floor_pct {
+            if self.land_latched {
+                return BatteryAction::None;
+            }
+            self.land_latched = true;
+            // Land-now also implies RTL is moot — latch RTL too so we
+            // do not double-fire on the next tick.
+            self.rtl_latched = true;
+            return BatteryAction::IssueLandNow;
+        }
+
+        if pct <= self.config.rtl_threshold_pct {
+            if self.rtl_latched {
+                return BatteryAction::None;
+            }
+            if self.override_active(now) {
+                return BatteryAction::SuppressedByOverride;
+            }
+            self.rtl_latched = true;
+            return BatteryAction::IssueRtl;
+        }
+
+        BatteryAction::None
+    }
+}
+
+/// Broadcast event for downstream observers (`operator_bridge` UI,
+/// future `shared::audit`).
+#[derive(Debug, Clone)]
+#[non_exhaustive]
+pub enum BatteryEvent {
+    OverrideApplied {
+        operator_id: String,
+        rationale: String,
+    },
+    RtlIssued,
+    LandNowIssued,
+    RtlSuppressedByOverride,
+}
+
+/// Pluggable command issuer; separate from the lost-link issuer per
+/// the AZ-651 "each failsafe family owns its command surface" pattern.
+#[async_trait]
+pub trait BatteryCommandIssuer: Send + Sync {
+    async fn issue_rtl(&self) -> Result<(), AutopilotError>;
+    async fn issue_land_now(&self) -> Result<(), AutopilotError>;
+}
+
+/// Production `BatteryCommandIssuer` backed by `mavlink_layer`. RTL
+/// is `MAV_CMD_NAV_RETURN_TO_LAUNCH` (same id used by the lost-link
+/// driver); land-now is `MAV_CMD_NAV_LAND` issued to the configured
+/// airframe with all `param_*` zeroed (let the airframe pick the
+/// safest reachable landing point per `architecture.md §7.7`).
+#[derive(Debug, Clone)]
+pub struct MavlinkBatteryCommandIssuer {
+    pub handle: MavlinkHandle,
+    pub target_system: u8,
+    pub target_component: u8,
+    pub ack_deadline: Option<Duration>,
+}
+
+impl MavlinkBatteryCommandIssuer {
+    pub fn new(handle: MavlinkHandle, target_system: u8, target_component: u8) -> Self {
+        Self {
+            handle,
+            target_system,
+            target_component,
+            ack_deadline: None,
+        }
+    }
+
+    async fn issue(&self, command: u16, what: &'static str) -> Result<(), AutopilotError> {
+        let cmd = CommandLong {
+            param1: 0.0,
+            param2: 0.0,
+            param3: 0.0,
+            param4: 0.0,
+            param5: 0.0,
+            param6: 0.0,
+            param7: 0.0,
+            command,
+            target_system: self.target_system,
+            target_component: self.target_component,
+            confirmation: 0,
+        };
+        self.handle
+            .send_command(cmd, self.ack_deadline)
+            .await
+            .map(|_ack| ())
+            .map_err(|e| match e {
+                SendCommandError::Timeout(d) => {
+                    AutopilotError::Internal(format!("battery {what} ack timeout after {d:?}"))
+                }
+                SendCommandError::Duplicate(id) => AutopilotError::Internal(format!(
+                    "battery {what} duplicate in flight (id={id})"
+                )),
+                SendCommandError::ChannelClosed(reason) => {
+                    AutopilotError::Internal(format!("battery {what} channel closed: {reason}"))
+                }
+            })
+    }
+}
+
+#[async_trait]
+impl BatteryCommandIssuer for MavlinkBatteryCommandIssuer {
+    async fn issue_rtl(&self) -> Result<(), AutopilotError> {
+        self.issue(MAV_CMD_NAV_RETURN_TO_LAUNCH, "RTL").await
+    }
+
+    async fn issue_land_now(&self) -> Result<(), AutopilotError> {
+        self.issue(MAV_CMD_NAV_LAND, "land-now").await
+    }
+}
+
+/// Public read-side handle.
+#[derive(Debug, Clone)]
+pub struct BatteryMonitorHandle {
+    events_tx: broadcast::Sender<BatteryEvent>,
+    last_action_rx: watch::Receiver<BatteryAction>,
+    override_tx: tokio::sync::mpsc::Sender<BatteryOverride>,
+}
+
+impl BatteryMonitorHandle {
+    pub fn subscribe(&self) -> broadcast::Receiver<BatteryEvent> {
+        self.events_tx.subscribe()
+    }
+
+    pub fn last_action(&self) -> BatteryAction {
+        *self.last_action_rx.borrow()
+    }
+
+    /// Apply a signed operator override. Returns `Err` if the driver
+    /// task has terminated.
+    pub async fn apply_override(&self, override_: BatteryOverride) -> Result<(), AutopilotError> {
+        self.override_tx
+            .send(override_)
+            .await
+            .map_err(|e| AutopilotError::Internal(format!("battery override channel closed: {e}")))
+    }
+}
+
+/// Driver — owns the monitor and ticks it from the telemetry
+/// `sys_status` watch.
+pub struct BatteryDriver<C: BatteryCommandIssuer + 'static> {
+    monitor: BatteryMonitor,
+    executor: MissionExecutorHandle,
+    command_issuer: Arc<C>,
+    sys_status_rx: watch::Receiver<Option<UavSysStatus>>,
+    tick_interval: Duration,
+}
+
+impl<C: BatteryCommandIssuer + 'static> BatteryDriver<C> {
+    pub fn new(
+        monitor: BatteryMonitor,
+        executor: MissionExecutorHandle,
+        command_issuer: Arc<C>,
+        sys_status_rx: watch::Receiver<Option<UavSysStatus>>,
+    ) -> Self {
+        Self {
+            monitor,
+            executor,
+            command_issuer,
+            sys_status_rx,
+            tick_interval: Duration::from_millis(100),
+        }
+    }
+
+    pub fn with_tick_interval(mut self, interval: Duration) -> Self {
+        self.tick_interval = interval;
+        self
+    }
+
+    pub fn spawn(
+        self,
+        mut shutdown: watch::Receiver<bool>,
+    ) -> (BatteryMonitorHandle, JoinHandle<()>) {
+        let (events_tx, _events_rx) = broadcast::channel::<BatteryEvent>(64);
+        let (action_tx, action_rx) = watch::channel(BatteryAction::None);
+        let (override_tx, mut override_rx) = tokio::sync::mpsc::channel::<BatteryOverride>(8);
+
+        let handle = BatteryMonitorHandle {
+            events_tx: events_tx.clone(),
+            last_action_rx: action_rx,
+            override_tx,
+        };
+
+        let BatteryDriver {
+            mut monitor,
+            executor,
+            command_issuer,
+            mut sys_status_rx,
+            tick_interval,
+        } = self;
+
+        let join = tokio::spawn(async move {
+            let mut ticker =
+                tokio::time::interval_at(Instant::now() + tick_interval, tick_interval);
+            ticker.set_missed_tick_behavior(tokio::time::MissedTickBehavior::Skip);
+            loop {
+                tokio::select! {
+                    biased;
+                    _ = shutdown.changed() => {
+                        tracing::info!("battery driver shutdown");
+                        return;
+                    }
+                    Some(o) = override_rx.recv() => {
+                        let op = o.operator_id.clone();
+                        let rationale = o.rationale.clone();
+                        monitor.apply_override(o);
+                        let _ = events_tx.send(BatteryEvent::OverrideApplied {
+                            operator_id: op,
+                            rationale,
+                        });
+                    }
+                    _ = ticker.tick() => {
+                        let sys_status_snapshot = *sys_status_rx.borrow_and_update();
+                        let Some(sys_status) = sys_status_snapshot else { continue };
+                        let now = Instant::now();
+                        let action = monitor.tick(&sys_status, now);
+                        let _ = action_tx.send(action);
+                        match action {
+                            BatteryAction::None => {}
+                            BatteryAction::SuppressedByOverride => {
+                                tracing::info!(
+                                    pct = sys_status.battery_remaining,
+                                    "battery RTL suppressed by operator override"
+                                );
+                                let _ = events_tx.send(BatteryEvent::RtlSuppressedByOverride);
+                            }
+                            BatteryAction::IssueRtl => {
+                                tracing::warn!(
+                                    pct = sys_status.battery_remaining,
+                                    "battery RTL threshold reached; issuing RTL"
+                                );
+                                if let Err(e) = command_issuer.issue_rtl().await {
+                                    tracing::error!(error=%e, "battery RTL command failed");
+                                }
+                                if let Err(e) = executor
+                                    .failsafe_trigger(FailsafeKind::BatteryRtl)
+                                    .await
+                                {
+                                    tracing::error!(error=%e, "battery executor failsafe_trigger(BatteryRtl) failed");
+                                }
+                                let _ = events_tx.send(BatteryEvent::RtlIssued);
+                            }
+                            BatteryAction::IssueLandNow => {
+                                tracing::error!(
+                                    pct = sys_status.battery_remaining,
+                                    "battery hard floor reached; issuing land-now"
+                                );
+                                if let Err(e) = command_issuer.issue_land_now().await {
+                                    tracing::error!(error=%e, "battery land-now command failed");
+                                }
+                                if let Err(e) = executor
+                                    .failsafe_trigger(FailsafeKind::BatteryHardFloor)
+                                    .await
+                                {
+                                    tracing::error!(error=%e, "battery executor failsafe_trigger(BatteryHardFloor) failed");
+                                }
+                                let _ = events_tx.send(BatteryEvent::LandNowIssued);
+                            }
+                        }
+                    }
+                }
+            }
+        });
+
+        (handle, join)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn sys_status(pct: i8) -> UavSysStatus {
+        UavSysStatus {
+            voltage_battery_mv: 12_000,
+            current_battery_ca: 100,
+            battery_remaining: pct,
+            onboard_sensors_health: 0,
+            errors_comm: 0,
+        }
+    }
+
+    #[test]
+    fn unknown_reading_is_no_action() {
+        // Arrange
+        let mut m = BatteryMonitor::new(BatteryConfig::default());
+
+        // Act
+        let a = m.tick(&sys_status(-1), Instant::now());
+
+        // Assert
+        assert_eq!(a, BatteryAction::None);
+    }
+
+    #[test]
+    fn above_threshold_is_no_action() {
+        // Arrange
+        let mut m = BatteryMonitor::new(BatteryConfig::default());
+
+        // Act
+        let a = m.tick(&sys_status(30), Instant::now());
+
+        // Assert
+        assert_eq!(a, BatteryAction::None);
+    }
+
+    #[test]
+    fn at_rtl_threshold_triggers_rtl_once() {
+        // Arrange
+        let mut m = BatteryMonitor::new(BatteryConfig::default());
+
+        // Act — first tick fires, second tick is latched
+        let a1 = m.tick(&sys_status(24), Instant::now());
+        let a2 = m.tick(&sys_status(23), Instant::now());
+
+        // Assert
+        assert_eq!(a1, BatteryAction::IssueRtl);
+        assert_eq!(a2, BatteryAction::None);
+    }
+
+    #[test]
+    fn at_hard_floor_triggers_land_now_once() {
+        // Arrange
+        let mut m = BatteryMonitor::new(BatteryConfig::default());
+
+        // Act
+        let a1 = m.tick(&sys_status(14), Instant::now());
+        let a2 = m.tick(&sys_status(10), Instant::now());
+
+        // Assert
+        assert_eq!(a1, BatteryAction::IssueLandNow);
+        assert_eq!(a2, BatteryAction::None);
+    }
+
+    #[test]
+    fn hard_floor_dominates_rtl_in_a_single_tick() {
+        // Arrange — battery dropped past both thresholds between ticks
+        let mut m = BatteryMonitor::new(BatteryConfig::default());
+
+        // Act
+        let a = m.tick(&sys_status(10), Instant::now());
+
+        // Assert — land-now, not RTL
+        assert_eq!(a, BatteryAction::IssueLandNow);
+    }
+
+    #[test]
+    fn active_override_suppresses_rtl_only() {
+        // Arrange
+        let mut m = BatteryMonitor::new(BatteryConfig::default());
+        let now = Instant::now();
+        m.apply_override(BatteryOverride {
+            until: now + Duration::from_secs(60),
+            operator_id: "op-1".into(),
+            rationale: "test".into(),
+        });
+
+        // Act — at RTL threshold, override should suppress
+        let a_rtl = m.tick(&sys_status(20), now);
+        // Reset latch so the hard-floor scenario is independent.
+        m.reset_latches();
+        // Hard floor is NEVER overridable
+        let a_land = m.tick(&sys_status(10), now);
+
+        // Assert
+        assert_eq!(a_rtl, BatteryAction::SuppressedByOverride);
+        assert_eq!(a_land, BatteryAction::IssueLandNow);
+    }
+
+    #[test]
+    fn expired_override_no_longer_suppresses() {
+        // Arrange
+        let mut m = BatteryMonitor::new(BatteryConfig::default());
+        let t0 = Instant::now();
+        m.apply_override(BatteryOverride {
+            until: t0 + Duration::from_millis(50),
+            operator_id: "op-1".into(),
+            rationale: "test".into(),
+        });
+
+        // Act — well after override expires
+        let later = t0 + Duration::from_secs(1);
+        let a = m.tick(&sys_status(20), later);
+
+        // Assert
+        assert_eq!(a, BatteryAction::IssueRtl);
+    }
+}

crates/mission_executor/src/internal/geofence.rs

@@ -0,0 +1,468 @@
+//! AZ-652 — geofence enforcement (INCLUSION + EXCLUSION).
+//!
+//! Symmetric semantics per the task spec: INCLUSION exit and EXCLUSION
+//! entry are both faults that must trigger RTL within ≤500 ms. The
+//! earlier C++ behaviour silently ignored EXCLUSION; the new design
+//! rejects that.
+//!
+//! The monitor is **pure logic**: `evaluate(pos, geofences)` is
+//! deterministic and side-effect-free. The driver in
+//! [`GeofenceDriver`] is the wiring layer that subscribes to a
+//! position stream, ticks the monitor, calls
+//! [`MissionExecutorHandle::failsafe_trigger`] on violation, and
+//! issues `MAV_CMD_NAV_RETURN_TO_LAUNCH` via the supplied command
+//! issuer. Following AZ-651's separation pattern, each failsafe family
+//! owns its own command-issuer trait (see
+//! [`crate::internal::lost_link`] for the lost-link variant).
+
+use std::sync::Arc;
+use std::time::Duration;
+
+use async_trait::async_trait;
+use mavlink_layer::{CommandLong, MavlinkHandle, SendCommandError};
+use tokio::sync::{broadcast, watch};
+use tokio::task::JoinHandle;
+use tokio::time::Instant;
+
+use shared::error::AutopilotError;
+use shared::models::mission::{Coordinate, Geofence, GeofenceKind};
+use shared::models::telemetry::UavPosition;
+
+use crate::internal::lost_link::MAV_CMD_NAV_RETURN_TO_LAUNCH;
+use crate::FailsafeKind;
+use crate::MissionExecutorHandle;
+
+/// Outcome of a single tick.
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum GeofenceVerdict {
+    /// Position satisfies every geofence (inside every INCLUSION,
+    /// outside every EXCLUSION).
+    Ok,
+    /// Position has exited an INCLUSION polygon.
+    InclusionExit,
+    /// Position has entered an EXCLUSION polygon.
+    ExclusionEntry,
+}
+
+impl GeofenceVerdict {
+    pub fn is_violation(self) -> bool {
+        !matches!(self, GeofenceVerdict::Ok)
+    }
+
+    pub fn failsafe_kind(self) -> Option<FailsafeKind> {
+        match self {
+            GeofenceVerdict::Ok => None,
+            GeofenceVerdict::InclusionExit => Some(FailsafeKind::GeofenceInclusion),
+            GeofenceVerdict::ExclusionEntry => Some(FailsafeKind::GeofenceExclusion),
+        }
+    }
+}
+
+/// Pure point-in-polygon evaluator for a fixed set of geofences.
+///
+/// Construction is cheap (no preprocessing); each `evaluate` call is
+/// O(total vertices). With the operational `≤8` geofences × `≤32`
+/// vertices typical for a single mission this is a few hundred
+/// floating-point ops per tick — comfortably under the AZ-652
+/// ≤500 ms response budget at the 10 Hz monitor cadence.
+#[derive(Debug, Clone)]
+pub struct GeofenceMonitor {
+    geofences: Vec<Geofence>,
+}
+
+impl GeofenceMonitor {
+    pub fn new(geofences: Vec<Geofence>) -> Self {
+        Self { geofences }
+    }
+
+    pub fn geofence_count(&self) -> usize {
+        self.geofences.len()
+    }
+
+    /// Evaluate the position against every fence. Returns the first
+    /// violation encountered (inclusion-exit checked first so a UAV
+    /// dropping past an inclusion boundary surfaces the more typical
+    /// fault first).
+    pub fn evaluate(&self, position: &UavPosition) -> GeofenceVerdict {
+        let point = Coordinate {
+            latitude: position.lat_e7 as f64 * 1.0e-7,
+            longitude: position.lon_e7 as f64 * 1.0e-7,
+            altitude_m: position.alt_m,
+        };
+        for fence in &self.geofences {
+            let inside = point_in_polygon(&point, &fence.vertices);
+            match (fence.kind, inside) {
+                (GeofenceKind::Inclusion, false) => return GeofenceVerdict::InclusionExit,
+                (GeofenceKind::Exclusion, true) => return GeofenceVerdict::ExclusionEntry,
+                _ => {}
+            }
+        }
+        GeofenceVerdict::Ok
+    }
+}
+
+/// Ray-casting point-in-polygon. The polygon is treated as closed
+/// (last vertex connects back to the first). Boundary semantics are
+/// "boundary counts as inside" — flying exactly along a fence line is
+/// considered compliant; the next tick that strays will surface the
+/// violation.
+fn point_in_polygon(point: &Coordinate, polygon: &[Coordinate]) -> bool {
+    if polygon.len() < 3 {
+        // Degenerate polygon — be safe: an INCLUSION with fewer than
+        // 3 vertices means "no valid inside" → caller treats as exit
+        // immediately. An EXCLUSION with fewer than 3 vertices is
+        // unenforceable; treat as outside (no entry possible).
+        return false;
+    }
+    let x = point.longitude;
+    let y = point.latitude;
+    let mut inside = false;
+    let n = polygon.len();
+    for i in 0..n {
+        let a = &polygon[i];
+        let b = &polygon[(i + 1) % n];
+        let (xi, yi) = (a.longitude, a.latitude);
+        let (xj, yj) = (b.longitude, b.latitude);
+        let crosses = (yi > y) != (yj > y) && {
+            // Avoid division by zero when the edge is horizontal —
+            // such an edge cannot be crossed by a horizontal ray.
+            let dy = yj - yi;
+            if dy.abs() < f64::EPSILON {
+                false
+            } else {
+                let x_at_y = (xj - xi) * (y - yi) / dy + xi;
+                x < x_at_y
+            }
+        };
+        if crosses {
+            inside = !inside;
+        }
+    }
+    inside
+}
+
+/// Broadcast event surfaced on every state transition or RTL trigger.
+#[derive(Debug, Clone, Copy)]
+#[non_exhaustive]
+pub enum GeofenceEvent {
+    Violation { kind: FailsafeKind },
+    RtlIssued { kind: FailsafeKind },
+    RtlSendFailed { kind: FailsafeKind },
+}
+
+/// Pluggable command issuer. Production wires this to
+/// [`MavlinkGeofenceCommandIssuer`]; tests supply a spy. Separate
+/// from the lost-link issuer so each failsafe family owns its own
+/// command surface (mirrors the AZ-651 pattern).
+#[async_trait]
+pub trait GeofenceCommandIssuer: Send + Sync {
+    async fn issue_rtl(&self) -> Result<(), AutopilotError>;
+}
+
+/// Production `GeofenceCommandIssuer` backed by `mavlink_layer`.
+/// Issues `MAV_CMD_NAV_RETURN_TO_LAUNCH` (same command id the
+/// lost-link path uses) targeting the configured airframe.
+#[derive(Debug, Clone)]
+pub struct MavlinkGeofenceCommandIssuer {
+    pub handle: MavlinkHandle,
+    pub target_system: u8,
+    pub target_component: u8,
+    pub ack_deadline: Option<Duration>,
+}
+
+impl MavlinkGeofenceCommandIssuer {
+    pub fn new(handle: MavlinkHandle, target_system: u8, target_component: u8) -> Self {
+        Self {
+            handle,
+            target_system,
+            target_component,
+            ack_deadline: None,
+        }
+    }
+}
+
+#[async_trait]
+impl GeofenceCommandIssuer for MavlinkGeofenceCommandIssuer {
+    async fn issue_rtl(&self) -> Result<(), AutopilotError> {
+        let cmd = CommandLong {
+            param1: 0.0,
+            param2: 0.0,
+            param3: 0.0,
+            param4: 0.0,
+            param5: 0.0,
+            param6: 0.0,
+            param7: 0.0,
+            command: MAV_CMD_NAV_RETURN_TO_LAUNCH,
+            target_system: self.target_system,
+            target_component: self.target_component,
+            confirmation: 0,
+        };
+        self.handle
+            .send_command(cmd, self.ack_deadline)
+            .await
+            .map(|_ack| ())
+            .map_err(|e| match e {
+                SendCommandError::Timeout(d) => AutopilotError::Internal(format!(
+                    "geofence RTL command ack timeout after {d:?}"
+                )),
+                SendCommandError::Duplicate(id) => AutopilotError::Internal(format!(
+                    "geofence RTL command duplicate in flight (id={id})"
+                )),
+                SendCommandError::ChannelClosed(reason) => AutopilotError::Internal(format!(
+                    "geofence RTL command channel closed: {reason}"
+                )),
+            })
+    }
+}
+
+/// Public read-side handle.
+#[derive(Debug, Clone)]
+pub struct GeofenceMonitorHandle {
+    events_tx: broadcast::Sender<GeofenceEvent>,
+    last_verdict_rx: watch::Receiver<GeofenceVerdict>,
+}
+
+impl GeofenceMonitorHandle {
+    pub fn subscribe(&self) -> broadcast::Receiver<GeofenceEvent> {
+        self.events_tx.subscribe()
+    }
+
+    pub fn last_verdict(&self) -> GeofenceVerdict {
+        *self.last_verdict_rx.borrow()
+    }
+}
+
+/// Driver — ticks the monitor against an incoming `UavPosition`
+/// stream and dispatches RTL on violation.
+pub struct GeofenceDriver<C: GeofenceCommandIssuer + 'static> {
+    monitor: GeofenceMonitor,
+    executor: MissionExecutorHandle,
+    command_issuer: Arc<C>,
+    position_rx: watch::Receiver<Option<UavPosition>>,
+    tick_interval: Duration,
+}
+
+impl<C: GeofenceCommandIssuer + 'static> GeofenceDriver<C> {
+    pub fn new(
+        monitor: GeofenceMonitor,
+        executor: MissionExecutorHandle,
+        command_issuer: Arc<C>,
+        position_rx: watch::Receiver<Option<UavPosition>>,
+    ) -> Self {
+        Self {
+            monitor,
+            executor,
+            command_issuer,
+            position_rx,
+            // 100 ms tick → ≤500 ms detect-to-RTL with healthy
+            // ground-station latency.
+            tick_interval: Duration::from_millis(100),
+        }
+    }
+
+    pub fn with_tick_interval(mut self, interval: Duration) -> Self {
+        self.tick_interval = interval;
+        self
+    }
+
+    /// Spawn the driver task and return the read-side handle plus the
+    /// task's join handle.
+    pub fn spawn(
+        self,
+        mut shutdown: watch::Receiver<bool>,
+    ) -> (GeofenceMonitorHandle, JoinHandle<()>) {
+        let (events_tx, _events_rx) = broadcast::channel::<GeofenceEvent>(64);
+        let (verdict_tx, verdict_rx) = watch::channel(GeofenceVerdict::Ok);
+        let handle = GeofenceMonitorHandle {
+            events_tx: events_tx.clone(),
+            last_verdict_rx: verdict_rx,
+        };
+
+        let GeofenceDriver {
+            monitor,
+            executor,
+            command_issuer,
+            mut position_rx,
+            tick_interval,
+        } = self;
+
+        let join = tokio::spawn(async move {
+            let mut ticker =
+                tokio::time::interval_at(Instant::now() + tick_interval, tick_interval);
+            ticker.set_missed_tick_behavior(tokio::time::MissedTickBehavior::Skip);
+            let mut last_verdict = GeofenceVerdict::Ok;
+            loop {
+                tokio::select! {
+                    biased;
+                    _ = shutdown.changed() => {
+                        tracing::info!("geofence driver shutdown");
+                        return;
+                    }
+                    _ = ticker.tick() => {
+                        let pos_snapshot = *position_rx.borrow_and_update();
+                        let Some(position) = pos_snapshot else {
+                            // No position yet — cannot evaluate.
+                            continue;
+                        };
+                        let verdict = monitor.evaluate(&position);
+                        let _ = verdict_tx.send(verdict);
+                        let entering_violation =
+                            verdict.is_violation() && !last_verdict.is_violation();
+                        last_verdict = verdict;
+                        if !entering_violation {
+                            continue;
+                        }
+                        let Some(kind) = verdict.failsafe_kind() else { continue };
+                        let _ = events_tx.send(GeofenceEvent::Violation { kind });
+                        tracing::warn!(
+                            ?kind,
+                            "geofence violation; issuing RTL"
+                        );
+                        match command_issuer.issue_rtl().await {
+                            Ok(()) => {
+                                let _ = events_tx.send(GeofenceEvent::RtlIssued { kind });
+                            }
+                            Err(e) => {
+                                tracing::error!(error=%e, ?kind, "geofence RTL send failed");
+                                let _ = events_tx.send(GeofenceEvent::RtlSendFailed { kind });
+                            }
+                        }
+                        if let Err(e) = executor.failsafe_trigger(kind).await {
+                            tracing::error!(error=%e, ?kind, "geofence executor.failsafe_trigger failed");
+                        }
+                    }
+                }
+            }
+        });
+
+        (handle, join)
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn coord(lat: f64, lon: f64) -> Coordinate {
+        Coordinate {
+            latitude: lat,
+            longitude: lon,
+            altitude_m: 0.0,
+        }
+    }
+
+    fn square_inclusion() -> Geofence {
+        Geofence {
+            kind: GeofenceKind::Inclusion,
+            vertices: vec![
+                coord(50.0, 30.0),
+                coord(50.0, 31.0),
+                coord(51.0, 31.0),
+                coord(51.0, 30.0),
+            ],
+        }
+    }
+
+    fn square_exclusion() -> Geofence {
+        Geofence {
+            kind: GeofenceKind::Exclusion,
+            vertices: vec![
+                coord(50.4, 30.4),
+                coord(50.4, 30.6),
+                coord(50.6, 30.6),
+                coord(50.6, 30.4),
+            ],
+        }
+    }
+
+    fn pos_at(lat: f64, lon: f64) -> UavPosition {
+        UavPosition {
+            lat_e7: (lat * 1.0e7) as i32,
+            lon_e7: (lon * 1.0e7) as i32,
+            alt_m: 100.0,
+            relative_alt_m: 50.0,
+            vx_mps: 0.0,
+            vy_mps: 0.0,
+            vz_mps: 0.0,
+            heading_deg: 0.0,
+            ts_boot_ms: 0,
+        }
+    }
+
+    #[test]
+    fn inclusion_inside_is_ok() {
+        // Arrange
+        let m = GeofenceMonitor::new(vec![square_inclusion()]);
+
+        // Act
+        let v = m.evaluate(&pos_at(50.5, 30.5));
+
+        // Assert
+        assert_eq!(v, GeofenceVerdict::Ok);
+    }
+
+    #[test]
+    fn inclusion_outside_is_exit() {
+        // Arrange
+        let m = GeofenceMonitor::new(vec![square_inclusion()]);
+
+        // Act
+        let v = m.evaluate(&pos_at(52.0, 30.5));
+
+        // Assert
+        assert_eq!(v, GeofenceVerdict::InclusionExit);
+        assert_eq!(v.failsafe_kind(), Some(FailsafeKind::GeofenceInclusion));
+    }
+
+    #[test]
+    fn exclusion_outside_is_ok() {
+        // Arrange
+        let m = GeofenceMonitor::new(vec![square_inclusion(), square_exclusion()]);
+
+        // Act — inside INCLUSION, outside EXCLUSION
+        let v = m.evaluate(&pos_at(50.2, 30.2));
+
+        // Assert
+        assert_eq!(v, GeofenceVerdict::Ok);
+    }
+
+    #[test]
+    fn exclusion_inside_is_entry() {
+        // Arrange
+        let m = GeofenceMonitor::new(vec![square_inclusion(), square_exclusion()]);
+
+        // Act — inside both INCLUSION and EXCLUSION
+        let v = m.evaluate(&pos_at(50.5, 30.5));
+
+        // Assert
+        assert_eq!(v, GeofenceVerdict::ExclusionEntry);
+        assert_eq!(v.failsafe_kind(), Some(FailsafeKind::GeofenceExclusion));
+    }
+
+    #[test]
+    fn degenerate_polygon_inclusion_is_exit() {
+        // Arrange — fewer than 3 vertices
+        let fence = Geofence {
+            kind: GeofenceKind::Inclusion,
+            vertices: vec![coord(0.0, 0.0), coord(1.0, 0.0)],
+        };
+
+        // Act
+        let v = GeofenceMonitor::new(vec![fence]).evaluate(&pos_at(0.5, 0.5));
+
+        // Assert
+        assert_eq!(v, GeofenceVerdict::InclusionExit);
+    }
+
+    #[test]
+    fn no_geofences_is_ok() {
+        // Arrange
+        let m = GeofenceMonitor::new(vec![]);
+
+        // Act
+        let v = m.evaluate(&pos_at(50.5, 30.5));
+
+        // Assert
+        assert_eq!(v, GeofenceVerdict::Ok);
+    }
+}

crates/mission_executor/src/internal/middle_waypoint.rs

@@ -0,0 +1,239 @@
+//! AZ-652 — middle-waypoint re-upload + target-follow resume.
+//!
+//! Two operations:
+//!
+//! 1. **Middle-waypoint insert** — operator confirms a POI; the
+//!    planner patches the active mission so the airframe diverts to
+//!    the confirmed target, then resumes the original route. The
+//!    `MISSION_CLEAR_ALL → upload all waypoints → MISSION_SET_CURRENT(0)`
+//!    sequence is delegated to
+//!    [`MissionDriver::upload_mission`](crate::MissionDriver::upload_mission),
+//!    which already implements that protocol as one atomic step.
+//!
+//! 2. **Target-follow release** — when target-follow ends (operator
+//!    explicitly releases, target lost, or timeout), the planner
+//!    recomputes the original mission from the current position and
+//!    re-uploads it.
+//!
+//! The *strategic* selection of where the middle waypoint lives in
+//! geographic terms is excluded from this task — `scan_controller`
+//! supplies that decision as a [`MiddleWaypointHint`]. This module
+//! owns the mechanics: building the patched mission vector and
+//! issuing the upload.
+
+use std::sync::Arc;
+
+use shared::models::mission::{Coordinate, MavCommand, MavFrame, MissionWaypoint};
+
+use crate::internal::driver::{DriverError, MissionDriver};
+
+/// Operator-confirmed POI handed to the planner.
+///
+/// The vertical / horizontal positioning of `at` is the strategic
+/// decision owned by `scan_controller`; the planner uses it
+/// verbatim. `insert_after_seq` identifies which existing waypoint
+/// the new one should follow; the existing waypoints with `seq >
+/// insert_after_seq` shift up by one.
+#[derive(Debug, Clone)]
+pub struct MiddleWaypointHint {
+    pub at: Coordinate,
+    pub insert_after_seq: u16,
+    /// Free-text label propagated for observability; not part of the
+    /// upload protocol.
+    pub label: Option<String>,
+}
+
+/// Re-planner. Holds a `MissionDriver` reference so the actual
+/// upload happens through the same protocol path the FSM uses.
+pub struct MissionRePlanner {
+    driver: Arc<dyn MissionDriver>,
+}
+
+impl MissionRePlanner {
+    pub fn new(driver: Arc<dyn MissionDriver>) -> Self {
+        Self { driver }
+    }
+
+    /// Patch the current mission with the middle-waypoint hint and
+    /// upload. Returns the patched mission so the caller can keep its
+    /// in-memory copy in sync with what the airframe now holds.
+    pub async fn on_middle_waypoint(
+        &self,
+        hint: MiddleWaypointHint,
+        current_mission: &[MissionWaypoint],
+    ) -> Result<Vec<MissionWaypoint>, DriverError> {
+        let patched = insert_middle_waypoint(current_mission, &hint);
+        self.driver.upload_mission(&patched).await?;
+        Ok(patched)
+    }
+
+    /// Recompute the original mission from `current_position` and
+    /// re-upload. Used when target-follow ends and the airframe must
+    /// resume its planned route from wherever it currently is.
+    ///
+    /// `original_mission` is the mission the airframe was flying
+    /// before target-follow took over. The recomputed mission prepends
+    /// a waypoint at `current_position` so the airframe has a smooth
+    /// rejoin point.
+    pub async fn on_target_follow_release(
+        &self,
+        original_mission: &[MissionWaypoint],
+        current_position: Coordinate,
+    ) -> Result<Vec<MissionWaypoint>, DriverError> {
+        let resume = recompute_resume(original_mission, &current_position);
+        self.driver.upload_mission(&resume).await?;
+        Ok(resume)
+    }
+}
+
+/// Construct a `MissionWaypoint` at `at` for use as a middle waypoint
+/// or rejoin point. Frame is `GLOBAL_RELATIVE_ALT`; command is
+/// `NAV_WAYPOINT`; `current` is `false` (the upload protocol's
+/// `MISSION_SET_CURRENT(0)` decides which waypoint becomes current);
+/// `auto_continue` is `true`.
+fn waypoint_at(seq: u16, at: &Coordinate) -> MissionWaypoint {
+    MissionWaypoint {
+        seq,
+        frame: MavFrame::MavFrameGlobalRelativeAlt,
+        command: MavCommand::MavCmdNavWaypoint,
+        current: false,
+        auto_continue: true,
+        param_1: 0.0,
+        param_2: 0.0,
+        param_3: 0.0,
+        param_4: 0.0,
+        lat_deg_e7: (at.latitude * 1.0e7) as i32,
+        lon_deg_e7: (at.longitude * 1.0e7) as i32,
+        alt_m: at.altitude_m,
+    }
+}
+
+/// Insert the middle waypoint after `hint.insert_after_seq`, shifting
+/// the subsequent waypoints' `seq` by one to preserve ordering.
+pub(crate) fn insert_middle_waypoint(
+    current: &[MissionWaypoint],
+    hint: &MiddleWaypointHint,
+) -> Vec<MissionWaypoint> {
+    // Find the split index. If the hint targets a sequence number
+    // past the end, append; if before the start, prepend.
+    let split_pos = current
+        .iter()
+        .position(|wp| wp.seq > hint.insert_after_seq)
+        .unwrap_or(current.len());
+
+    let mut patched: Vec<MissionWaypoint> = Vec::with_capacity(current.len() + 1);
+    patched.extend_from_slice(&current[..split_pos]);
+    patched.push(waypoint_at(0, &hint.at));
+    patched.extend_from_slice(&current[split_pos..]);
+
+    // Renumber so `seq` is contiguous starting from 0.
+    for (i, wp) in patched.iter_mut().enumerate() {
+        wp.seq = i as u16;
+    }
+    patched
+}
+
+/// Build the resume mission: a single rejoin waypoint at
+/// `current_position` followed by the original waypoints with
+/// renumbered `seq`. The rejoin waypoint becomes index 0 so
+/// `MISSION_SET_CURRENT(0)` (issued by the upload protocol) targets
+/// it first.
+pub(crate) fn recompute_resume(
+    original: &[MissionWaypoint],
+    current_position: &Coordinate,
+) -> Vec<MissionWaypoint> {
+    let mut resume: Vec<MissionWaypoint> = Vec::with_capacity(original.len() + 1);
+    resume.push(waypoint_at(0, current_position));
+    for (i, wp) in original.iter().enumerate() {
+        let mut next = *wp;
+        next.seq = (i + 1) as u16;
+        resume.push(next);
+    }
+    resume
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    fn wp(seq: u16, lat: f64, lon: f64) -> MissionWaypoint {
+        MissionWaypoint {
+            seq,
+            frame: MavFrame::MavFrameGlobalRelativeAlt,
+            command: MavCommand::MavCmdNavWaypoint,
+            current: false,
+            auto_continue: true,
+            param_1: 0.0,
+            param_2: 0.0,
+            param_3: 0.0,
+            param_4: 0.0,
+            lat_deg_e7: (lat * 1.0e7) as i32,
+            lon_deg_e7: (lon * 1.0e7) as i32,
+            alt_m: 50.0,
+        }
+    }
+
+    fn coord(lat: f64, lon: f64) -> Coordinate {
+        Coordinate {
+            latitude: lat,
+            longitude: lon,
+            altitude_m: 50.0,
+        }
+    }
+
+    #[test]
+    fn insert_in_the_middle_shifts_subsequent_seqs() {
+        // Arrange
+        let mission = vec![wp(0, 50.0, 30.0), wp(1, 50.1, 30.1), wp(2, 50.2, 30.2)];
+        let hint = MiddleWaypointHint {
+            at: coord(50.05, 30.05),
+            insert_after_seq: 0,
+            label: Some("poi-1".into()),
+        };
+
+        // Act
+        let patched = insert_middle_waypoint(&mission, &hint);
+
+        // Assert
+        assert_eq!(patched.len(), 4);
+        let seqs: Vec<u16> = patched.iter().map(|w| w.seq).collect();
+        assert_eq!(seqs, vec![0, 1, 2, 3]);
+        // The inserted waypoint is index 1 (after the seq-0 waypoint).
+        assert_eq!(patched[1].lat_deg_e7, (50.05 * 1.0e7) as i32);
+        assert_eq!(patched[1].lon_deg_e7, (30.05 * 1.0e7) as i32);
+    }
+
+    #[test]
+    fn insert_past_end_appends() {
+        // Arrange
+        let mission = vec![wp(0, 50.0, 30.0), wp(1, 50.1, 30.1)];
+        let hint = MiddleWaypointHint {
+            at: coord(60.0, 40.0),
+            insert_after_seq: 99,
+            label: None,
+        };
+
+        // Act
+        let patched = insert_middle_waypoint(&mission, &hint);
+
+        // Assert
+        assert_eq!(patched.len(), 3);
+        assert_eq!(patched.last().unwrap().lat_deg_e7, (60.0 * 1.0e7) as i32);
+    }
+
+    #[test]
+    fn recompute_resume_prepends_current_position() {
+        // Arrange
+        let mission = vec![wp(0, 50.0, 30.0), wp(1, 50.1, 30.1)];
+
+        // Act
+        let resume = recompute_resume(&mission, &coord(50.05, 30.05));
+
+        // Assert
+        assert_eq!(resume.len(), 3);
+        let seqs: Vec<u16> = resume.iter().map(|w| w.seq).collect();
+        assert_eq!(seqs, vec![0, 1, 2]);
+        assert_eq!(resume[0].lat_deg_e7, (50.05 * 1.0e7) as i32);
+        assert_eq!(resume[1].lat_deg_e7, (50.0 * 1.0e7) as i32);
+    }
+}

crates/mission_executor/src/internal/mod.rs

@@ -1,11 +1,15 @@
 //! Internal modules for `mission_executor`. Not part of the public API.
 
+pub mod battery_thresholds;
 pub mod bit;
 pub mod bit_evaluators;
 pub mod driver;
 pub mod fixed_wing;
 pub mod fsm;
+pub mod geofence;
 pub mod lost_link;
+pub mod middle_waypoint;
 pub mod multirotor;
+pub mod post_flight;
 pub mod telemetry;
 pub mod types;

crates/mission_executor/src/internal/post_flight.rs

@@ -0,0 +1,171 @@
+//! AZ-652 — post-flight MapObjects push trigger (F8).
+//!
+//! On entry to `MissionState::PostFlightSync` the executor must hand
+//! off to `mission_client::push_mapobjects_diff(mission_id, diff)`.
+//! The push itself is best-effort: `mission_client` (AZ-647) owns
+//! the write-ahead persistence and the retry budget, so even if the
+//! call returns a `Degraded` `PushReport` the executor must reach
+//! `MissionState::Done`. A persistently failing push surfaces a
+//! manual-replay warning via `mission_client` health, not a stuck FSM.
+//!
+//! ## Test seam
+//!
+//! Production wires [`MissionClientHandle`] directly (the blanket
+//! impl below makes it satisfy [`MapObjectsPusher`]); tests inject a
+//! spy implementing the same trait so call counts and inputs can be
+//! asserted without spinning up an HTTP client.
+
+use std::sync::atomic::{AtomicU64, Ordering};
+use std::sync::Arc;
+
+use async_trait::async_trait;
+
+use mission_client::{MapObjectsDiff, MissionClientHandle, PushReport};
+
+/// What the post-flight pusher needs from the world. A trait — not
+/// the concrete `MissionClientHandle` — so tests can inject a spy.
+#[async_trait]
+pub trait MapObjectsPusher: Send + Sync {
+    async fn push(&self, mission_id: &str, diff: MapObjectsDiff) -> PushReport;
+}
+
+#[async_trait]
+impl MapObjectsPusher for MissionClientHandle {
+    async fn push(&self, mission_id: &str, diff: MapObjectsDiff) -> PushReport {
+        self.push_mapobjects_diff(mission_id, diff).await
+    }
+}
+
+/// Where the pending mapobjects diff comes from at post-flight time.
+/// `mapobjects_store` (AZ-667/668) owns this; tests substitute a
+/// closure-backed source.
+#[async_trait]
+pub trait MapObjectsDiffSource: Send + Sync {
+    async fn drain_diff(&self) -> MapObjectsDiff;
+}
+
+/// Orchestrates one post-flight push. Stateless aside from a push
+/// counter used by health.
+pub struct PostFlightPusher<P: MapObjectsPusher, S: MapObjectsDiffSource> {
+    pusher: Arc<P>,
+    diff_source: Arc<S>,
+    push_count: AtomicU64,
+}
+
+impl<P: MapObjectsPusher, S: MapObjectsDiffSource> PostFlightPusher<P, S> {
+    pub fn new(pusher: Arc<P>, diff_source: Arc<S>) -> Self {
+        Self {
+            pusher,
+            diff_source,
+            push_count: AtomicU64::new(0),
+        }
+    }
+
+    pub fn push_count(&self) -> u64 {
+        self.push_count.load(Ordering::Relaxed)
+    }
+
+    /// Push exactly once. Returns the report so the caller can surface
+    /// per-endpoint status; even a `Degraded` report is "success" from
+    /// the FSM's standpoint — the FSM must reach `Done` regardless.
+    /// The persistence and retry of the failing endpoint is owned by
+    /// [`MissionClientHandle::push_mapobjects_diff`] (AZ-647).
+    pub async fn push(&self, mission_id: &str) -> PushReport {
+        let diff = self.diff_source.drain_diff().await;
+        let report = self.pusher.push(mission_id, diff).await;
+        self.push_count.fetch_add(1, Ordering::Relaxed);
+        let sync_state = report.sync_state();
+        match sync_state {
+            mission_client::SyncState::Synced => {
+                tracing::info!(
+                    mission_id = %mission_id,
+                    "post-flight mapobjects push completed (synced)"
+                );
+            }
+            mission_client::SyncState::Degraded => {
+                tracing::warn!(
+                    mission_id = %mission_id,
+                    "post-flight mapobjects push degraded; mission_client retains pending payload for manual replay"
+                );
+            }
+        }
+        report
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use mission_client::PerEndpointStatus;
+    use std::sync::Mutex;
+
+    #[derive(Default)]
+    struct SpyPusher {
+        calls: Mutex<Vec<(String, MapObjectsDiff)>>,
+        /// What to return.
+        report_template: Mutex<Option<PushReport>>,
+    }
+
+    #[async_trait]
+    impl MapObjectsPusher for SpyPusher {
+        async fn push(&self, mission_id: &str, diff: MapObjectsDiff) -> PushReport {
+            self.calls
+                .lock()
+                .unwrap()
+                .push((mission_id.to_owned(), diff.clone()));
+            self.report_template
+                .lock()
+                .unwrap()
+                .clone()
+                .unwrap_or_else(|| PushReport {
+                    mission_id: mission_id.to_owned(),
+                    observations: PerEndpointStatus::Success,
+                    ignored: PerEndpointStatus::Success,
+                })
+        }
+    }
+
+    struct EmptyDiffSource;
+    #[async_trait]
+    impl MapObjectsDiffSource for EmptyDiffSource {
+        async fn drain_diff(&self) -> MapObjectsDiff {
+            MapObjectsDiff::default()
+        }
+    }
+
+    #[tokio::test]
+    async fn push_invokes_pusher_once_and_increments_counter() {
+        // Arrange
+        let spy = Arc::new(SpyPusher::default());
+        let p = PostFlightPusher::new(spy.clone(), Arc::new(EmptyDiffSource));
+
+        // Act
+        let _report = p.push("M1").await;
+
+        // Assert
+        assert_eq!(spy.calls.lock().unwrap().len(), 1);
+        assert_eq!(spy.calls.lock().unwrap()[0].0, "M1");
+        assert_eq!(p.push_count(), 1);
+    }
+
+    #[tokio::test]
+    async fn degraded_report_still_returns_normally() {
+        // Arrange
+        let spy = Arc::new(SpyPusher::default());
+        *spy.report_template.lock().unwrap() = Some(PushReport {
+            mission_id: "M1".into(),
+            observations: PerEndpointStatus::Success,
+            ignored: PerEndpointStatus::Permanent {
+                reason: "503 budget".into(),
+            },
+        });
+        let p = PostFlightPusher::new(spy.clone(), Arc::new(EmptyDiffSource));
+
+        // Act
+        let report = p.push("M1").await;
+
+        // Assert — FSM must still reach Done even on degraded outcome.
+        assert_eq!(report.sync_state(), mission_client::SyncState::Degraded);
+        assert_eq!(p.push_count(), 1);
+    }
+}

crates/mission_executor/src/lib.rs

@@ -18,6 +18,7 @@
 //!   on cap exhaustion the FSM moves to [`MissionState::Paused`] and
 //!   health flips to red.
 
+use std::sync::atomic::{AtomicBool, Ordering};
 use std::sync::Arc;
 use std::time::Duration;
 
@@ -32,6 +33,11 @@ use shared::models::mission::{Coordinate, MissionItem, MissionWaypoint};
 
 mod internal;
 
+pub use internal::battery_thresholds::{
+    BatteryAction, BatteryCommandIssuer, BatteryConfig, BatteryDriver, BatteryEvent,
+    BatteryMonitor, BatteryMonitorHandle, BatteryOverride, MavlinkBatteryCommandIssuer,
+    MAV_CMD_NAV_LAND,
+};
 pub use internal::bit::{
     BitController, BitControllerConfig, BitControllerHandle, BitDegradedAck, BitEvaluator,
     BitEvent, BitItem, BitItemStatus, BitOverall, BitReport, BitState,
@@ -41,11 +47,17 @@ pub use internal::bit_evaluators::{
     WallClockBoundEvaluator,
 };
 pub use internal::driver::{DriverError, MissionDriver};
+pub use internal::geofence::{
+    GeofenceCommandIssuer, GeofenceDriver, GeofenceEvent, GeofenceMonitor, GeofenceMonitorHandle,
+    GeofenceVerdict, MavlinkGeofenceCommandIssuer,
+};
 pub use internal::lost_link::{
     LadderEvent, LadderInput, LadderOutput, LadderState, LostLinkCommandIssuer, LostLinkConfig,
     LostLinkDriver, LostLinkLadder, LostLinkLadderHandle, MavlinkCommandIssuer,
     MAV_CMD_NAV_RETURN_TO_LAUNCH,
 };
+pub use internal::middle_waypoint::{MiddleWaypointHint, MissionRePlanner};
+pub use internal::post_flight::{MapObjectsDiffSource, MapObjectsPusher, PostFlightPusher};
 pub use internal::telemetry::{
     Consumer, DropCountingReceiver, MavlinkProjection, TelemetryForwarder,
 };
@@ -167,6 +179,8 @@ impl MissionExecutor {
         let handle = MissionExecutorHandle {
             core: core.clone(),
             events_tx: events_tx.clone(),
+            driver: driver_for_task.clone(),
+            hard_floor_active: Arc::new(AtomicBool::new(false)),
         };
 
         let join = tokio::spawn(async move {
@@ -207,6 +221,13 @@ async fn run_loop(
 pub struct MissionExecutorHandle {
     core: Arc<Mutex<FsmCore>>,
     events_tx: broadcast::Sender<TransitionEvent>,
+    /// Driver used by [`insert_middle_waypoint`] and any other
+    /// failsafe path that needs to issue a fresh mission upload.
+    driver: Arc<dyn MissionDriver>,
+    /// Set to `true` once the battery hard floor (15 % default) has
+    /// fired. Latched: only the operator-level recovery flow can
+    /// clear it. Drives `health()` → red while active.
+    hard_floor_active: Arc<AtomicBool>,
 }
 
 impl MissionExecutorHandle {
@@ -232,9 +253,25 @@ impl MissionExecutorHandle {
         self.core.lock().await.paused_reason.clone()
     }
 
-    /// Aggregated health: red when paused, green when `Done`,
-    /// yellow otherwise.
+    /// `true` once the battery hard floor (15 % default) has fired.
+    /// Drives `health()` → red until cleared via
+    /// [`MissionExecutorHandle::clear_hard_floor`].
+    pub fn hard_floor_active(&self) -> bool {
+        self.hard_floor_active.load(Ordering::Relaxed)
+    }
+
+    /// Operator-acknowledged clear of the hard-floor latch. Intended
+    /// for ground-test workflows where the battery has been swapped.
+    pub fn clear_hard_floor(&self) {
+        self.hard_floor_active.store(false, Ordering::Relaxed);
+    }
+
+    /// Aggregated health: red when paused or while the battery hard
+    /// floor has fired, green when `Done`, yellow otherwise.
     pub async fn health(&self) -> ComponentHealth {
+        if self.hard_floor_active() {
+            return ComponentHealth::red(NAME, "battery hard floor active");
+        }
         let guard = self.core.lock().await;
         match guard.state {
             MissionState::Paused => {
@@ -249,66 +286,102 @@ impl MissionExecutorHandle {
         }
     }
 
-    /// 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)",
-        ))
+    /// Insert a single middle waypoint immediately after the
+    /// currently-active waypoint (or, if the mission has not started
+    /// yet, at the head) and re-upload via the driver. Returns once
+    /// the airframe has acknowledged the new mission. Strategic
+    /// placement decisions (where in geographic space the new
+    /// waypoint belongs) are owned by `scan_controller`; this entry
+    /// point handles the **mechanics** of patch + re-upload only.
+    pub async fn insert_middle_waypoint(&self, at: Coordinate) -> Result<()> {
+        let hint = MiddleWaypointHint {
+            at,
+            // Insert after seq 0 so the airframe still treats seq 0
+            // as the rejoin anchor. scan_controller will eventually
+            // supply a richer hint via a follow-up surface.
+            insert_after_seq: 0,
+            label: None,
+        };
+        let current_mission: Vec<MissionWaypoint> = {
+            let guard = self.core.lock().await;
+            guard.mission.clone()
+        };
+        let planner = MissionRePlanner::new(self.driver.clone());
+        let patched = planner
+            .on_middle_waypoint(hint, &current_mission)
+            .await
+            .map_err(|e| AutopilotError::Internal(format!("middle-waypoint re-upload: {e}")))?;
+        let mut guard = self.core.lock().await;
+        guard.mission = patched;
+        Ok(())
     }
 
     /// Apply a failsafe response immediately.
     ///
-    /// AZ-651 implements the link-loss family: `LinkLost` and
-    /// `LinkLostInFollow` both cause the FSM to short-circuit from
-    /// `FlyMission` to `Land` (and the lost-link driver issues
-    /// `MAV_CMD_NAV_RETURN_TO_LAUNCH` separately so the airframe also
-    /// returns home — the FSM transition reflects the autopilot's
-    /// internal accounting). Other states are NOT overridden: if the
-    /// FSM is still in `Disconnected` / `Armed` / `TakeOff` /
-    /// `MissionUploaded`, the airframe failsafe is the right authority
-    /// and we let it handle the abort.
+    /// All non-degraded variants short-circuit `MissionState::FlyMission`
+    /// → `MissionState::Land`. The actual MAVLink command
+    /// (`MAV_CMD_NAV_RETURN_TO_LAUNCH` or `MAV_CMD_NAV_LAND`) is
+    /// issued by the dedicated driver for each failsafe family
+    /// (`LostLinkDriver` for `LinkLost*`, `BatteryDriver` for
+    /// `BatteryRtl` / `BatteryHardFloor`, `GeofenceDriver` for the
+    /// geofence variants). The FSM transition recorded here is the
+    /// autopilot's internal accounting of the abort; the airframe
+    /// follows the command sent by the driver.
     ///
-    /// Battery and geofence failsafes (`BatteryRtl`, `BatteryHardFloor`,
-    /// `GeofenceInclusion`, `GeofenceExclusion`) land in AZ-652 with
-    /// their own state-aware overrides; calling this method with one
-    /// of those kinds returns `NotImplemented` for now.
+    /// Earlier states (`Disconnected`, `Connected`, `HealthOk`,
+    /// `BitOk`, `Armed`, `TakeOff`, `MissionUploaded`) are NOT
+    /// overridden: in those states the airframe's own failsafe and
+    /// the driver's command are the right authority.
     ///
-    /// Calling this while the FSM is already `Paused` is a no-op (we
-    /// do not clobber the existing pause).
+    /// Calling this while the FSM is already `Paused` is a no-op.
     pub async fn failsafe_trigger(&self, kind: FailsafeKind) -> Result<()> {
         match kind {
-            FailsafeKind::LinkLost | FailsafeKind::LinkLostInFollow => {
-                let mut core = self.core.lock().await;
-                if core.state == MissionState::FlyMission {
-                    let from = core.state;
-                    core.state = MissionState::Land;
-                    let _ = self.events_tx.send(TransitionEvent {
-                        variant: core.variant,
-                        from,
-                        to: MissionState::Land,
-                        at: chrono::Utc::now(),
-                        retry_count: 0,
-                    });
-                }
-                // Other states (incl. Paused) — leave alone. The
-                // airframe's own failsafe (or whatever paused us) is
-                // authoritative.
-                Ok(())
-            }
             FailsafeKind::LinkDegraded => {
                 // Degraded is yellow-health-only; no transition needed.
                 Ok(())
             }
-            FailsafeKind::BatteryRtl
-            | FailsafeKind::BatteryHardFloor
+            FailsafeKind::LinkLost
+            | FailsafeKind::LinkLostInFollow
+            | FailsafeKind::BatteryRtl
             | FailsafeKind::GeofenceInclusion
-            | FailsafeKind::GeofenceExclusion => Err(AutopilotError::NotImplemented(
-                "mission_executor::failsafe_trigger: battery/geofence land in AZ-652",
-            )),
+            | FailsafeKind::GeofenceExclusion => {
+                self.transition_flymission_to_land().await;
+                Ok(())
+            }
+            FailsafeKind::BatteryHardFloor => {
+                self.hard_floor_active.store(true, Ordering::Relaxed);
+                self.transition_flymission_to_land().await;
+                Ok(())
+            }
         }
     }
 
+    async fn transition_flymission_to_land(&self) {
+        let mut core = self.core.lock().await;
+        if core.state == MissionState::FlyMission {
+            let from = core.state;
+            core.state = MissionState::Land;
+            let _ = self.events_tx.send(TransitionEvent {
+                variant: core.variant,
+                from,
+                to: MissionState::Land,
+                at: chrono::Utc::now(),
+                retry_count: 0,
+            });
+        }
+    }
+
+    /// Test-only back-door for forcing FSM state. The FSM normally
+    /// advances through telemetry-gated transitions; integration
+    /// tests that need to assert failsafe behaviour in a specific
+    /// state use this rather than wiring a full transition harness.
+    /// Not part of the production API.
+    #[doc(hidden)]
+    pub async fn force_state_for_tests(&self, state: MissionState) {
+        let mut core = self.core.lock().await;
+        core.state = state;
+    }
+
     /// 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<()> {