protoflight 0.1.4

Protoflight flight controller.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

#![cfg(feature = "autopilot")]

#[cfg(any(feature = "gps", feature = "optical_flow"))]
use sensor_fusion::KalmanFilterXYZWithSensorsf32;
use sensor_fusion::KalmanFilterZWithSensorsf32;

use super::dual_ring_pid_vertical::MultirotorAltitudeDualRingPid;

#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Autopilot {
    pub altitude_controller: MultirotorAltitudeDualRingPid,
    pub altitude_kalman_filter: KalmanFilterZWithSensorsf32,
    #[cfg(any(feature = "gps", feature = "optical_flow"))]
    pub position_kalman_filter: KalmanFilterXYZWithSensorsf32,
}

impl Default for Autopilot {
    fn default() -> Self {
        Self::new()
    }
}

impl Autopilot {
    pub fn new() -> Self {
        Self {
            altitude_controller: MultirotorAltitudeDualRingPid::new(0.0),
            altitude_kalman_filter: KalmanFilterZWithSensorsf32::new(),
            #[cfg(any(feature = "gps", feature = "optical_flow"))]
            position_kalman_filter: KalmanFilterXYZWithSensorsf32::new(),
        }
    }
}