autopilot/crates/mission_executor/src/internal/battery_thresholds.rs

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