protoflight 0.1.3

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

#[allow(unused)]
use radio_controllers::{Rates, RcModes, RcSticks, RxFrame};
use simple_bitset::BitSet64;

/// Message for communicating a radio control command between tasks.<br><br>
#[derive(Clone, Copy, Debug, PartialEq)]
#[repr(C)]
pub struct RcControls {
    pub tick_count: u32,
    pub throttle_stick: f32,
    pub roll_stick_dps: f32,
    pub pitch_stick_dps: f32,
    pub yaw_stick_dps: f32,
    pub roll_stick_degrees: f32,
    pub pitch_stick_degrees: f32,
    pub controls_pwm: [u16; 4],
    pub failsafe: u8,
}
const _: () = assert!(core::mem::size_of::<RcControls>() == 40);

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

impl RcControls {
    pub const fn new() -> Self {
        Self {
            tick_count: 0,
            throttle_stick: 0.0,
            roll_stick_dps: 0.0,
            pitch_stick_dps: 0.0,
            yaw_stick_dps: 0.0,
            roll_stick_degrees: 0.0,
            pitch_stick_degrees: 0.0,
            controls_pwm: [0u16; 4],
            failsafe: 0,
        }
    }
}

/// Message for communicating a radio control command between tasks.<br><br>
#[derive(Clone, Copy, Debug, PartialEq)]
#[repr(C)]
pub struct RxMessage {
    pub rc_modes: BitSet64,
    pub rc_controls: RcControls,
}
const _: () = assert!(core::mem::size_of::<RxMessage>() == 48);

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

impl RxMessage {
    pub const fn new() -> Self {
        Self { rc_modes: BitSet64::new(), rc_controls: RcControls::new() }
    }
}

impl RxMessage {
    /// Create a `RadioControlMessage` from an `RxFrame`, applying rates and including `RcModes`.
    pub fn new_from(rx_frame: &RxFrame, rates: &Rates, rc_modes: &RcModes, tick_count: u32, failsafe: u8) -> RxMessage {
        // get the stick values from the rx_frame.
        let sticks = RcSticks::from(*rx_frame);

        // apply rates to the stick values.
        let roll_stick_dps = rates.apply(Rates::ROLL, sticks.roll);
        let pitch_stick_dps = rates.apply(Rates::PITCH, sticks.pitch);
        let yaw_stick_dps = rates.apply(Rates::YAW, sticks.yaw);

        // scale the stick angles.
        let roll_stick_degrees = sticks.roll * rates.max_roll_angle_degrees;
        let pitch_stick_degrees = sticks.pitch * rates.max_pitch_angle_degrees;

        // Get the rc_modes (eg altitude hold, gps home) (used by the autopilot),
        // and the stabilization mode (eg STABILIZATION_MODE_RATE) (used by the flight controller).

        RxMessage {
            rc_modes: rc_modes.active_modes,
            rc_controls: RcControls {
                tick_count,

                throttle_stick: sticks.throttle,
                roll_stick_dps,
                pitch_stick_dps,
                yaw_stick_dps,
                roll_stick_degrees,
                pitch_stick_degrees,
                controls_pwm: [rx_frame.channels[0], rx_frame.channels[1], rx_frame.channels[2], rx_frame.channels[3]],

                failsafe,
            },
        }
    }
}

#[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::<RxMessage>();
    }
    #[test]
    fn sizeof() {
        assert_eq!(48, core::mem::size_of::<RxMessage>());
    }
}