flight-computer 0.1.0

//! # Flight phase
//!
//! This module deals with determining which phase of flight the device is in,
//! as well as managing the altimeter.

#[cfg(not(test))]
use micromath::F32Ext;

use crate::{
    altimeter::Altimeter,
    ublox_device::{Device, UbloxDriver},
    utils::WheelBuffer,
};
use embedded_hal::blocking::delay::DelayMs;
use fugit::{ExtU64, MillisDuration, MillisDurationU32};

const BUFFER_LENGTH: usize = 5;
/// Barometric data picked up at a specific timestamp
// TODO make BaroData non-Copy
#[derive(Debug, Clone, Copy)]
pub struct BaroData {
    /// Altitude above ground in meters
    pub agl: f32,
    /// Pressure in Pa
    pub pressure: f32,
    /// Timestamp in ms
    pub timestamp: MillisDuration<u64>,
}

impl core::default::Default for BaroData {
    fn default() -> Self {
        Self {
            timestamp: 0.millis(),
            ..BaroData::default()
        }
    }
}

impl core::ops::Sub<BaroData> for BaroData {
    type Output = Self;

    #[inline]
    fn sub(self, rhs: Self) -> Self::Output {
        BaroData {
            agl: self.agl - rhs.agl,
            pressure: self.pressure - rhs.pressure,
            timestamp: self.timestamp - rhs.timestamp,
        }
    }
}

/// Used to detect the current flight phase
pub struct BaroComputer<A: Altimeter> {
    /// Altitude measuring device
    altimeter: A,
    /// Buffer holding the last `BUFFER_LENGTH` baro samples
    buf: WheelBuffer<BaroData, BUFFER_LENGTH>,
    /// The current flight phase
    current_phase: FlightPhase,
    /// Number of pre-trigger checks that have been passed
    num_pre_checks: usize,
    /// Internal ground reference. Most importantly, holds the ground pressure
    ground_ref: BaroData,
    /// Absolute latest measurement. May be more recent than the most recent
    /// measurement in `buf`.
    latest_measurement: BaroData,
}

impl<A: Altimeter> BaroComputer<A> {
    /// Create a new `BaroComputer`
    #[inline]
    pub fn new(altimeter: A, current_phase: FlightPhase, fill_buf: BaroData) -> Self {
        Self {
            altimeter,
            current_phase,
            buf: WheelBuffer::new(fill_buf),
            num_pre_checks: 0,
            ground_ref: fill_buf,
            latest_measurement: fill_buf,
        }
    }

    /// Return the current flight phase
    #[inline]
    pub fn current_phase(&self) -> FlightPhase {
        self.current_phase
    }

    /// Return the computer's ground reference.
    #[inline]
    pub fn ground_ref(&self) -> BaroData {
        self.ground_ref
    }

    /// Execute a barometric measurement. Will store in
    /// `self.latest_measurement`
    #[inline]
    pub fn measure<D: DelayMs<u8>>(&mut self, delay: &mut D, timestamp: MillisDuration<u64>) {
        self.latest_measurement = self.altimeter.measure(delay, self.ground_ref(), timestamp);
    }

    /// Return a shared reference to the computer's internal [`WheelBuffer`]
    #[inline]
    pub fn buf(&self) -> &WheelBuffer<BaroData, BUFFER_LENGTH> {
        &self.buf
    }

    /// Return the latest barometric measurement
    #[inline]
    pub fn last_measurement(&self) -> BaroData {
        self.latest_measurement
    }

    /// Notify that the GPS signal has been acquired, and that we can switch
    /// from `FlightPhase::Startup` to `FlightPhase::Ground`
    #[inline]
    pub fn acquire_signal<D: UbloxDriver, const L: usize>(&mut self, gps: &mut Device<D, L>) {
        gps.acquire_signal();

        if self.current_phase == FlightPhase::Startup {
            self.current_phase = FlightPhase::Ground;
            gps.gnss_power(false);
        }
    }

    /// Switch flight phases
    ///
    /// Compute whether the computer should switch to a new flight phase. Run a
    /// closure __only__ if the phase has switched.
    #[inline]
    pub fn check_phase_switch<F: FnMut(FlightPhase, BaroData, &mut A)>(
        &mut self,
        mut on_switch_phase: F,
    ) {
        self.push_latest();

        let old_phase = self.current_phase();

        match old_phase {
            FlightPhase::Startup | FlightPhase::Ground => {
                if self.detect_climb() {
                    self.current_phase = FlightPhase::Climb;
                }
            }

            FlightPhase::Climb => {
                if self.detect_freefall() {
                    self.current_phase = FlightPhase::FlightLogging;
                }
            }

            FlightPhase::FlightLogging => {
                if self.detect_landing() {
                    self.current_phase = FlightPhase::Ground;
                }
            }
        }

        let new_phase = self.current_phase();

        if old_phase != new_phase {
            on_switch_phase(new_phase, self.buf.latest(), &mut self.altimeter);
        }
    }

    /// Add a new measurement to the computer's internal [`WheelBuffer`]
    #[inline]
    fn push_latest(&mut self) {
        self.buf.push(self.last_measurement());
    }

    /// Compute the difference between the last two measurements in buffer
    #[inline]
    fn diff(&self) -> BaroData {
        self.buf.peek(0).unwrap() - self.buf.peek(1).unwrap()
    }

    /// Detect if we should switch to climb mode from ground mode
    #[inline]
    fn detect_climb(&mut self) -> bool {
        const REQD_PRE_CHECKS: usize = 2;
        // Rates are divided by 1000 (meters per millisecond) because we store the
        // timestamp as milliseconds
        const TRIGGER_CLIMB_RATES: [f32; REQD_PRE_CHECKS + 1] =
            [0.25 / 1000.0, 0.75 / 1000.0, 1.0 / 1000.0];
        static_assertions::const_assert!(REQD_PRE_CHECKS < BUFFER_LENGTH);

        let mut pre_trigger = false;

        let climb_detected = self.detect_phase(REQD_PRE_CHECKS, |diff, num_pre_checks| {
            pre_trigger =
                diff.agl >= TRIGGER_CLIMB_RATES[num_pre_checks] * diff.timestamp.ticks() as f32;

            pre_trigger
        });

        // Reset ground pressure only if there was no pre-trigger (no climb is
        // suspected)
        //
        // Also, we are a taking a ground pressure further back in time (8 * 4 = 32
        // seconds) to try to pick up a pressure which was valid *before* the aircraft
        // started rolling, which could mess with pressure measurements.
        if !pre_trigger {
            self.set_ground_ref(self.buf().peek(4).unwrap());
        }

        climb_detected
    }

    /// Detect if we should switch to ground mode from freefall mode
    #[inline]
    fn detect_landing(&mut self) -> bool {
        // Rates are divided by 1000 (meters per millisecond) because we store the
        // timestamp as milliseconds
        const LANDING_RATE_MAX: f32 = 3.0 / 1000.0;
        const REQD_PRE_CHECKS: usize = 19;

        self.detect_phase(REQD_PRE_CHECKS, |diff, _| {
            diff.agl.abs() <= LANDING_RATE_MAX * diff.timestamp.ticks() as f32
        })
    }

    /// Detect if we should switch to freefall mode from climb mod
    #[inline]
    fn detect_freefall(&mut self) -> bool {
        const REQD_PRE_CHECKS: usize = 3;
        // Rates are divided by 1000 (meters per millisecond) because we store the
        // timestamp as milliseconds
        const TRIGGER_FREEFALL_RATES: [f32; REQD_PRE_CHECKS + 1] =
            [-3.0 / 1000.0, -5.0 / 1000.0, -8.0 / 1000.0, -10.0 / 1000.0];

        self.detect_phase(REQD_PRE_CHECKS, |diff, num_pre_checks| {
            diff.agl <= TRIGGER_FREEFALL_RATES[num_pre_checks] * diff.timestamp.ticks() as f32
        })
    }

    /// # Check if we should switch flight phases.
    ///
    /// This function takes a closure that accepts a [`BaroData`] which
    /// represents the difference between the last and next last baro
    /// measurements.
    #[inline]
    fn detect_phase<F>(&mut self, reqd_pre_checks: usize, mut func: F) -> bool
    where
        F: FnMut(BaroData, usize) -> bool,
    {
        // TODO check that the timestamp difference is large enough (eg 8 seconds)
        // (unneeded if the time diff comes from the FlightPhase impl?)
        let diff = self.diff();

        // n-stage check: check was passed
        if func(diff, self.num_pre_checks) {
            // Final check: switch mode
            if self.num_pre_checks >= reqd_pre_checks {
                self.num_pre_checks = 0;
                return true;
                // This wasn't the final check; increment pre-trigger counter
            }

            self.num_pre_checks += 1;

        // Check wasn't passed; reset pre-trigger counter
        } else {
            self.num_pre_checks = 0;
        }

        false
    }

    /// Set the computer's internal ground reference
    #[inline]
    fn set_ground_ref(&mut self, ground_ref: BaroData) {
        self.ground_ref = ground_ref;
    }
}

/// The flight phase describes the current state of the device
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum FlightPhase {
    /// Device startup
    Startup,
    /// Active data logging in flight
    FlightLogging,
    /// On the ground; measure pressure to detect takeoff
    Ground,
    /// Climbing in aircraft; wait for exit
    Climb,
}

impl FlightPhase {
    /// Time after which a new phase check should be done (in ms)
    #[inline]
    pub const fn phase_check_wait_time(&self) -> MillisDuration<u32> {
        match self {
            FlightPhase::Startup | FlightPhase::Ground => MillisDurationU32::from_ticks(10_000),
            FlightPhase::FlightLogging | FlightPhase::Climb => MillisDurationU32::from_ticks(1000),
        }
    }

    pub const FLIGHT_LOGGING_MEASUREMENT_INTERVAL: MillisDuration<u32> =
        MillisDurationU32::from_ticks(50);
}