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

use simple_bitset::BitSet64;
use vqm::{Quaternionf32, Vector3f32, Vector4f32};
/*
Current Estimates for GyroPidMessage:
AccData (4x f32 ): 16 bytes
GyroData (4x f32 ): 16 bytes
GyroUnfiltered (4x f32 ): 16 bytes
Orientation 16 bytes
MotorCommands (4x u16): 8 bytes
Setpoints (3x f32): 12 bytes
PID Errors (3x f32): 12 bytes
 */

/// Blackbox telemetry data, updated at approximately 1kHz.
/// Limit this to 128 bytes.
/// On a 32-bit ARM processor, a memcpy of 128 bytes takes roughly 32 to 64 clock cycles.
/// At 200MHz, that is less than 0.4 microseconds.
/// Even with the overhead of the Watch mutex (critical section), this is well under 1μs of total time to dispatch that data.
#[derive(Clone, Copy, Debug, PartialEq)]
#[repr(C)]
pub struct GyroPidMessage {
    pub orientation: Quaternionf32,
    pub motor_commands: Vector4f32,
    pub acc: Vector3f32,
    pub gyro_rps: Vector3f32,
    pub gyro_rps_unfiltered: Vector3f32,
    pub pid_errors_p: [f32; Self::RPY_AXIS_COUNT],
    pub pid_errors_i: [f32; Self::RPY_AXIS_COUNT],
    pub pid_errors_d: [f32; Self::RP_AXIS_COUNT],
    pub time_us: u64,
}

//#[cfg(not(any(feature = "servos", feature = "eight_motors")))]
const _: () = assert!(core::mem::size_of::<GyroPidMessage>() == 112);

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

impl GyroPidMessage {
    pub const RPY_AXIS_COUNT: usize = 3;
    pub const RP_AXIS_COUNT: usize = 2;

    #[must_use]
    pub const fn new() -> Self {
        Self {
            orientation: Quaternionf32::new(0.0, 0.0, 0.0, 0.0),
            motor_commands: Vector4f32::new(0.0, 0.0, 0.0, 0.0),
            acc: Vector3f32::new(0.0, 0.0, 0.0),
            gyro_rps: Vector3f32::new(0.0, 0.0, 0.0),
            gyro_rps_unfiltered: Vector3f32::new(0.0, 0.0, 0.0),
            pid_errors_p: [0f32; Self::RPY_AXIS_COUNT],
            pid_errors_i: [0f32; Self::RPY_AXIS_COUNT],
            pid_errors_d: [0f32; Self::RP_AXIS_COUNT],
            time_us: 0,
        }
    }
}

/// Message to send setpoint data between tasks.
#[derive(Clone, Copy, Debug, PartialEq)]
#[repr(C)]
pub struct SetpointMessage {
    pub setpoints: [f32; Self::SETPOINT_COUNT],
    pub pid_errors_s: [f32; Self::RPY_AXIS_COUNT],
    pub pid_errors_k: [f32; Self::RPY_AXIS_COUNT],
    pub rc_commands: [u16; Self::RC_COMMAND_COUNT],
    #[cfg(feature = "dshot_telemetry")]
    pub motor_rpm_d2: [i16; Self::MAX_SUPPORTED_MOTOR_COUNT], // motor rpm divided by 2
    #[cfg(feature = "servos")]
    pub servos: [i16; Self::MAX_SUPPORTED_SERVO_COUNT],
    pub time_us: u64,
    pub rc_modes: BitSet64,
    pub gps_state_flags: u8,
    pub failsafe_phase: u8,
    pub rx_signal_received: bool,
    pub rx_flight_channel_is_valid: bool,
}

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

#[allow(unused)]
impl SetpointMessage {
    pub const RC_COMMAND_COUNT: usize = 4;
    pub const SETPOINT_COUNT: usize = 4;
    pub const THROTTLE: usize = 3;
    pub const RPY_AXIS_COUNT: usize = 3;

    #[cfg(feature = "eight_motors")]
    pub const MAX_SUPPORTED_MOTOR_COUNT: usize = 8;
    #[cfg(not(feature = "eight_motors"))]
    pub const MAX_SUPPORTED_MOTOR_COUNT: usize = 4;

    // ailerons, elevator, rudder, throttle (which may be controlled by a servo, if the motor is an internal combustion engine)
    #[cfg(feature = "servos")]
    pub const MAX_SUPPORTED_SERVO_COUNT: usize = 8;

    #[must_use]
    pub const fn new() -> Self {
        Self {
            setpoints: [0.0; Self::SETPOINT_COUNT],
            pid_errors_s: [0.0; Self::RPY_AXIS_COUNT],
            pid_errors_k: [0.0; Self::RPY_AXIS_COUNT],
            rc_commands: [1500, 1500, 1500, 1000],
            #[cfg(feature = "dshot_telemetry")]
            motor_rpm_d2: [0i16; Self::MAX_SUPPORTED_MOTOR_COUNT],
            #[cfg(feature = "servos")]
            servos: [0i16; Self::MAX_SUPPORTED_SERVO_COUNT],
            time_us: 0,
            rc_modes: BitSet64::new(),
            gps_state_flags: 0,
            failsafe_phase: 0,
            rx_signal_received: false,
            rx_flight_channel_is_valid: false,
        }
    }
}

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

    fn _is_normal<T: Sized + Send + Sync + Unpin>() {}
    fn is_full<T: Sized + Send + Sync + Unpin + Copy + Clone + Default + PartialEq>() {}

    #[test]
    fn normal_types() {
        is_full::<GyroPidMessage>();
        is_full::<SetpointMessage>();
    }
    #[test]
    fn sizeof() {
        assert_eq!(112, core::mem::size_of::<GyroPidMessage>());
        #[cfg(all(feature = "dshot_telemetry", not(any(feature = "servos", feature = "eight_motors"))))]
        assert_eq!(80, core::mem::size_of::<SetpointMessage>());
    }
    #[test]
    fn gyro_pid_message_new() {
        let telemetry_data = GyroPidMessage::default();
        assert_eq!(0, telemetry_data.time_us);
    }
}