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
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243

use crate::flight::PidConfig;
#[cfg(feature = "storage")]
use sequential_storage::map::PostcardValue;
#[cfg(feature = "serde")]
use {
    postcard::experimental::max_size::MaxSize,
    serde::{Deserialize, Serialize},
};

// RX Loss Policy: What to do when radio signal is lost during autopilot
pub struct AutopilotRxLoss {}

#[allow(unused)]
impl AutopilotRxLoss {
    pub const DISABLE: u8 = 0; // Disable autopilot, use standard failsafe
    pub const CONTINUE: u8 = 1; // Continue mission (if GPS valid)
    pub const LAND: u8 = 2; // Land at current position
}

pub struct AutopilotYawMode {}

#[allow(unused)]
impl AutopilotYawMode {
    pub const VELOCITY: u8 = 0; // Multirotor: point nose in velocity direction
    pub const BEARING: u8 = 1; // Multirotor: point nose at waypoint
    pub const HYBRID: u8 = 2; // Multirotor: blend based on distance
    pub const FIXED: u8 = 3; // Multirotor: no yaw control
    pub const DAMPENER: u8 = 4; // Wing: yaw rate damper (coordinated turns)
}

#[derive(Clone, Copy, Debug, PartialEq)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize, MaxSize))]
pub struct AutopilotConfig {
    pub landing_altitude_m: u8, // altitude below which landing behaviors can change, metres
    pub hover_throttle: u16,    // value used at the start of a rescue or position hold
    pub throttle_min: u16,
    pub throttle_max: u16,
    pub altitude_pid: PidConfig,
    pub position_pid: PidConfig,
    pub position_i_i: u8,
    pub position_a: u8,
    pub position_cutoff: u8,
    pub max_angle: u8,

    // Velocity-based position control with drag compensation
    pub velocity_control_enable: u8, // 0=position→angle, 1=position→velocity→angle (default 1)
    pub velocity_pid: PidConfig,
    pub velocity_drag_coeff: u16, // drag coefficient, scaled by 10000 (default 50 = 0.0050)
    pub max_velocity_cmps: u16,   // cm/s, maximum velocity setpoint (default 1000 = 10 m/s)

    // Waypoint navigation parameters
    pub waypoint_arrival_radius_cm: u16, // cm, arrival detection radius (default 500)
    pub waypoint_hold_radius_cm: u16,    // cm, hold pattern radius (default 200)
    pub stick_deadband: u16,             // RC units (0-500), deadband for pilot override (default 50)
    pub throttle_deadband: u16,          // RC units (0-500), throttle override deadband (default 50)

    // Yaw control parameters
    pub yaw_mode: u8,
    pub yaw_pid: PidConfig,             // scaled by 100 (e.g., 50 = 0.5)
    pub max_yaw_rate_dps: u16,          // deg/s, maximum yaw rate
    pub min_forward_velocity_cmps: u16, // cm/s, minimum forward velocity: G_pS course reliability (multirotor) / stall prevention (wing)

    // Velocity buildup (acceleration from stationary)
    pub velocity_buildup_max_pitch: u8, // degrees, max pitch bias for acceleration (default 8)

    // Turn rate and holding patterns
    pub max_turn_rate_dps: u8, // deg/s, maximum turn rate (default 3 for rate-1 turn, configurable 1-90)
    pub hold_orbit_radius_cm: u16, // cm, radius for orbit holding pattern (default 1000 = 10m)
    pub hold_figure8_width_cm: u16, // cm, width of figure-8 pattern (default 2000 = 20m)

    // Landing sequence
    pub landing_descent_rate: u8, // cm/s, descent rate during landing (default 50 = 0.5 m/s)
    pub landing_detection_time: u8, // deciseconds, time below threshold for touchdown detection (default 10 = 1s)
    pub landing_spiral_enable: u8, // 0=straight descent, 1=spiral (avoid vortex ring state, default 1)
    pub landing_spiral_radius: u16, // cm, radius of spiral pattern (default 200 = 2m)
    pub landing_spiral_rate: u8,  // deg/s, rotation rate (default 10 = 10 deg/s)
    pub landing_velocity_threshold: u8, // cm/s, max velocity for touchdown (default 50 = 0.5 m/s)
    pub landing_throttle_threshold: u16, // RC units, max throttle deviation for touchdown (default 100)

    // L1 Nonlinear Guidance parameters
    pub l1_enable: u8,                 // 0=direct targeting, 1=L1 guidance (default 1)
    pub l1_period: u16,                // L1 damping period, deciseconds (default 20 = 2.0s)
    pub l1_min_lookahead: u16,         // Minimum lookahead distance, cm (default 1000 = 10m)
    pub l1_max_lookahead: u16,         // Maximum lookahead distance, cm (default 10000 = 100m)
    pub l1_max_cross_track_error: u16, // Max cross-track error before fallback, cm (default 10000 = 100m)
    pub l1_turn_rate: u8, // deg/s, max turn rate for arc transitions between waypoints (default 8, 0=disabled)

    // Vertical track: climb to min altitude, then follow glide slope to waypoint
    pub min_nav_altitude_m: u8, // metres AGL, minimum altitude before horizontal navigation (default 5)

    // Safety: RX loss policy
    pub rx_loss_policy: u8,

    // Safety: geofence (max distance from home)
    pub max_distance_from_home_m: u8, // meters, 0 = disabled (default 0)
    pub geofence_action: u8,
}

#[cfg(feature = "storage")]
impl PostcardValue<'_> for AutopilotConfig {}

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

impl AutopilotConfig {
    pub const fn new() -> Self {
        Self {
            landing_altitude_m: 4,
            hover_throttle: 1275,
            throttle_min: 1100,
            throttle_max: 1900,
            altitude_pid: PidConfig { kp: 15, ki: 15, kd: 15, ks: 0, kk: 15 },
            position_pid: PidConfig { kp: 30, ki: 30, kd: 30, ks: 0, kk: 30 },
            position_i_i: 30,
            position_a: 30,
            position_cutoff: 30,
            max_angle: 50,

            velocity_control_enable: 1,
            velocity_pid: PidConfig { kp: 50, ki: 10, kd: 5, ks: 0, kk: 0 },
            velocity_drag_coeff: 50, // 0.0050 drag coefficient
            max_velocity_cmps: 1000, // 10 m/s max velocity

            waypoint_arrival_radius_cm: 500, // 5m for FLYOVER/FLYBY
            waypoint_hold_radius_cm: 200,    // 2m for HOLD/LAND
            stick_deadband: 50,              // RC units
            throttle_deadband: 50,           // RC units

            yaw_mode: AutopilotYawMode::VELOCITY,
            yaw_pid: PidConfig { kp: 0, ki: 0, kd: 0, ks: 0, kk: 0 },
            max_yaw_rate_dps: 0,
            min_forward_velocity_cmps: 0,

            velocity_buildup_max_pitch: 0,

            max_turn_rate_dps: 0,
            hold_orbit_radius_cm: 0,
            hold_figure8_width_cm: 0,

            landing_descent_rate: 0,
            landing_detection_time: 0,
            landing_spiral_enable: 0,
            landing_spiral_radius: 0,
            landing_spiral_rate: 0,
            landing_velocity_threshold: 0,
            landing_throttle_threshold: 0,

            l1_enable: 0,
            l1_period: 0,
            l1_min_lookahead: 0,
            l1_max_lookahead: 0,
            l1_max_cross_track_error: 0,
            l1_turn_rate: 0,

            min_nav_altitude_m: 0,

            rx_loss_policy: 0,

            max_distance_from_home_m: 0,
            geofence_action: 0,
        }
    }
}

#[derive(Clone, Copy, Debug, PartialEq)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize, MaxSize))]
pub struct PositionHoldConfig {
    pub deadband: u8,
    /// Position source selection.
    pub position_source: u8,
    /// Minimum optical flow quality threshold.
    pub optical_flow_quality_min: u8,
    /// Maximum altitude for optical flow (cm).
    pub optical_flow_max_range_cm: u16,
}

#[cfg(feature = "storage")]
impl PostcardValue<'_> for PositionHoldConfig {}

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

#[allow(unused)]
impl PositionHoldConfig {
    pub const SOURCE_AUTO: u8 = 0;
    pub const SOURCE_GPS_ONLY: u8 = 1;
    pub const SOURCE_OPTICAL_FLOW_ONLY: u8 = 2;

    pub const fn new() -> Self {
        Self { deadband: 0, position_source: 0, optical_flow_quality_min: 0, optical_flow_max_range_cm: 0 }
    }
}

#[cfg(test)]
mod test_traits {
    use super::*;

    fn _is_normal<T: Sized + Send + Sync + Unpin>() {}
    fn is_full<T: Sized + Send + Sync + Unpin + Copy + Clone + Default + PartialEq>() {}
    #[cfg(feature = "serde")]
    fn is_serde<T: Serialize + MaxSize + for<'a> Deserialize<'a>>() {}
    #[cfg(feature = "storage")]
    fn is_storage<T: for<'a> PostcardValue<'a>>() {}

    #[test]
    fn normal_types() {
        is_full::<AutopilotConfig>();
        is_full::<PositionHoldConfig>();
        #[cfg(feature = "serde")]
        is_serde::<AutopilotConfig>();
        #[cfg(feature = "serde")]
        is_serde::<PositionHoldConfig>();
    }
    #[cfg(feature = "serde")]
    #[test]
    fn serde_types() {
        is_serde::<AutopilotConfig>();
        is_serde::<PositionHoldConfig>();
    }
    #[cfg(feature = "storage")]
    #[test]
    fn storage_types() {
        is_storage::<AutopilotConfig>();
        is_storage::<PositionHoldConfig>();
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_new() {
        let config = AutopilotConfig::new();
        assert_eq!(4, config.landing_altitude_m);
    }
}