[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/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<()> {