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