148 lines
4.1 KiB
Rust
148 lines
4.1 KiB
Rust
//! Fail-safe state machine: link loss, low battery, collision avoidance.
|
|
|
|
use crate::types::DroneState;
|
|
use serde::{Deserialize, Serialize};
|
|
use std::time::Instant;
|
|
|
|
/// Fail-safe operating state.
|
|
#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
|
|
pub enum FailSafeState {
|
|
Nominal,
|
|
AutonomousHold,
|
|
LowBatteryWarn,
|
|
ReturnToHome,
|
|
EmergencyLand,
|
|
EmergencyDiverge,
|
|
ControlledDescent,
|
|
}
|
|
|
|
/// State machine driving fail-safe transitions.
|
|
pub struct FailSafeMachine {
|
|
state: FailSafeState,
|
|
link_loss_start: Option<Instant>,
|
|
pub link_loss_hold_secs: f64,
|
|
pub link_loss_rth_secs: f64,
|
|
pub battery_warn_pct: f32,
|
|
pub battery_rth_pct: f32,
|
|
pub collision_dist_m: f64,
|
|
}
|
|
|
|
impl FailSafeMachine {
|
|
pub fn new() -> Self {
|
|
Self {
|
|
state: FailSafeState::Nominal,
|
|
link_loss_start: None,
|
|
link_loss_hold_secs: 3.0,
|
|
link_loss_rth_secs: 30.0,
|
|
battery_warn_pct: 20.0,
|
|
battery_rth_pct: 15.0,
|
|
collision_dist_m: 1.5,
|
|
}
|
|
}
|
|
|
|
/// Drive one tick. Returns the current state after evaluation.
|
|
pub fn tick(
|
|
&mut self,
|
|
state: &DroneState,
|
|
link_alive: bool,
|
|
nearest_neighbor_dist: f64,
|
|
) -> FailSafeState {
|
|
// Collision avoidance has highest priority
|
|
if nearest_neighbor_dist < self.collision_dist_m {
|
|
self.state = FailSafeState::EmergencyDiverge;
|
|
return self.state.clone();
|
|
}
|
|
|
|
// Link loss handling
|
|
if !link_alive {
|
|
let start = self.link_loss_start.get_or_insert_with(Instant::now);
|
|
let elapsed = start.elapsed().as_secs_f64();
|
|
if elapsed > self.link_loss_rth_secs {
|
|
self.state = FailSafeState::ReturnToHome;
|
|
} else if elapsed > self.link_loss_hold_secs {
|
|
self.state = FailSafeState::AutonomousHold;
|
|
}
|
|
return self.state.clone();
|
|
} else {
|
|
// Link restored
|
|
self.link_loss_start = None;
|
|
if self.state == FailSafeState::AutonomousHold {
|
|
self.state = FailSafeState::Nominal;
|
|
}
|
|
}
|
|
|
|
// Battery checks
|
|
if state.battery_pct <= self.battery_rth_pct {
|
|
self.state = FailSafeState::ReturnToHome;
|
|
} else if state.battery_pct <= self.battery_warn_pct {
|
|
self.state = FailSafeState::LowBatteryWarn;
|
|
} else if self.state == FailSafeState::LowBatteryWarn {
|
|
// Recovered from low battery (charged on the fly / wrong reading)
|
|
self.state = FailSafeState::Nominal;
|
|
}
|
|
|
|
self.state.clone()
|
|
}
|
|
|
|
pub fn current(&self) -> &FailSafeState {
|
|
&self.state
|
|
}
|
|
|
|
pub fn force_land(&mut self) {
|
|
self.state = FailSafeState::EmergencyLand;
|
|
}
|
|
}
|
|
|
|
impl Default for FailSafeMachine {
|
|
fn default() -> Self {
|
|
Self::new()
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
use crate::types::NodeId;
|
|
|
|
fn good_state() -> DroneState {
|
|
let mut s = DroneState::default_at_origin(NodeId(1));
|
|
s.battery_pct = 80.0;
|
|
s.link_quality = 1.0;
|
|
s
|
|
}
|
|
|
|
#[test]
|
|
fn test_nominal_when_healthy() {
|
|
let mut fsm = FailSafeMachine::new();
|
|
let s = good_state();
|
|
let result = fsm.tick(&s, true, 10.0);
|
|
assert_eq!(result, FailSafeState::Nominal);
|
|
}
|
|
|
|
#[test]
|
|
fn test_low_battery_warn() {
|
|
let mut fsm = FailSafeMachine::new();
|
|
let mut s = good_state();
|
|
s.battery_pct = 18.0;
|
|
let result = fsm.tick(&s, true, 10.0);
|
|
assert_eq!(result, FailSafeState::LowBatteryWarn);
|
|
}
|
|
|
|
#[test]
|
|
fn test_battery_rth() {
|
|
let mut fsm = FailSafeMachine::new();
|
|
let mut s = good_state();
|
|
s.battery_pct = 10.0;
|
|
let result = fsm.tick(&s, true, 10.0);
|
|
assert_eq!(result, FailSafeState::ReturnToHome);
|
|
}
|
|
|
|
#[test]
|
|
fn test_collision_avoidance() {
|
|
let mut fsm = FailSafeMachine::new();
|
|
let s = good_state();
|
|
let result = fsm.tick(&s, true, 0.5); // too close
|
|
assert_eq!(result, FailSafeState::EmergencyDiverge);
|
|
}
|
|
}
|