126 lines
3.3 KiB
Rust
126 lines
3.3 KiB
Rust
//! Flight controller abstraction and simulated implementation.
|
|
|
|
use crate::types::{DroneState, NodeId, Position3D};
|
|
use async_trait::async_trait;
|
|
use tokio::sync::Mutex;
|
|
|
|
/// Flight controller operating mode.
|
|
#[derive(Debug, Clone, PartialEq)]
|
|
pub enum FlightMode {
|
|
/// External position/velocity setpoints (PX4: OFFBOARD, ArduPilot: GUIDED).
|
|
Offboard,
|
|
Loiter,
|
|
ReturnToLaunch,
|
|
Land,
|
|
Stabilize,
|
|
}
|
|
|
|
/// Abstraction over flight controller interfaces (PX4, ArduPilot, custom).
|
|
#[async_trait]
|
|
pub trait FlightController: Send + Sync {
|
|
async fn set_target_position(
|
|
&self,
|
|
pos: &Position3D,
|
|
speed_ms: f64,
|
|
) -> crate::SwarmResult<()>;
|
|
|
|
async fn get_state(&self) -> crate::SwarmResult<DroneState>;
|
|
|
|
async fn set_mode(&self, mode: FlightMode) -> crate::SwarmResult<()>;
|
|
|
|
async fn arm(&self) -> crate::SwarmResult<()>;
|
|
|
|
async fn disarm(&self) -> crate::SwarmResult<()>;
|
|
|
|
async fn rtl(&self) -> crate::SwarmResult<()>;
|
|
|
|
async fn emergency_land(&self) -> crate::SwarmResult<()>;
|
|
}
|
|
|
|
/// A simulated flight controller that immediately applies position commands.
|
|
/// Used in tests and demo mode.
|
|
pub struct SimulatedFlightController {
|
|
pub state: Mutex<DroneState>,
|
|
}
|
|
|
|
impl SimulatedFlightController {
|
|
pub fn new(id: NodeId) -> Self {
|
|
Self {
|
|
state: Mutex::new(DroneState::default_at_origin(id)),
|
|
}
|
|
}
|
|
}
|
|
|
|
#[async_trait]
|
|
impl FlightController for SimulatedFlightController {
|
|
async fn set_target_position(
|
|
&self,
|
|
pos: &Position3D,
|
|
_speed_ms: f64,
|
|
) -> crate::SwarmResult<()> {
|
|
let mut state = self.state.lock().await;
|
|
state.position = *pos;
|
|
Ok(())
|
|
}
|
|
|
|
async fn get_state(&self) -> crate::SwarmResult<DroneState> {
|
|
let state = self.state.lock().await;
|
|
Ok(state.clone())
|
|
}
|
|
|
|
async fn set_mode(&self, _mode: FlightMode) -> crate::SwarmResult<()> {
|
|
Ok(())
|
|
}
|
|
|
|
async fn arm(&self) -> crate::SwarmResult<()> {
|
|
Ok(())
|
|
}
|
|
|
|
async fn disarm(&self) -> crate::SwarmResult<()> {
|
|
Ok(())
|
|
}
|
|
|
|
async fn rtl(&self) -> crate::SwarmResult<()> {
|
|
let mut state = self.state.lock().await;
|
|
state.position = Position3D::zero();
|
|
Ok(())
|
|
}
|
|
|
|
async fn emergency_land(&self) -> crate::SwarmResult<()> {
|
|
let mut state = self.state.lock().await;
|
|
state.altitude_agl_m = 0.0;
|
|
state.position.z = 0.0;
|
|
Ok(())
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
#[tokio::test]
|
|
async fn test_set_position_updates_state() {
|
|
let fc = SimulatedFlightController::new(NodeId(0));
|
|
let target = Position3D { x: 50.0, y: 30.0, z: -20.0 };
|
|
fc.set_target_position(&target, 5.0).await.unwrap();
|
|
let state = fc.get_state().await.unwrap();
|
|
assert!((state.position.x - 50.0).abs() < 1e-6);
|
|
assert!((state.position.y - 30.0).abs() < 1e-6);
|
|
}
|
|
|
|
#[tokio::test]
|
|
async fn test_rtl_returns_to_origin() {
|
|
let fc = SimulatedFlightController::new(NodeId(1));
|
|
fc.set_target_position(
|
|
&Position3D { x: 100.0, y: 100.0, z: -30.0 },
|
|
5.0,
|
|
)
|
|
.await
|
|
.unwrap();
|
|
fc.rtl().await.unwrap();
|
|
let state = fc.get_state().await.unwrap();
|
|
assert!(state.position.x.abs() < 1e-6);
|
|
assert!(state.position.y.abs() < 1e-6);
|
|
}
|
|
}
|