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