veloxity_core/
pwm.rs

1use crate::{board::BoardIo, math::FlightFloat, mixer::MixerOutputType};
2
3pub mod output_sync;
4
5#[derive(Debug, Copy, Clone, PartialEq, Eq)]
6pub enum PwmError {
7    ChannelOutOfRange,
8    GenericError,
9    InvalidRate,
10    UnsupportedProtocol,
11}
12
13#[derive(Debug, Copy, Clone, PartialEq, Eq)]
14pub enum PwmOutputProtocol {
15    StandardPwm,
16    Dshot,
17}
18
19pub const STANDARD_PWM_DEFAULT_RATE_HZ: f32 = 50.0;
20
21#[derive(Debug, Copy, Clone, PartialEq, Eq)]
22pub struct DshotCommand {
23    pub throttle: u16,
24    pub telemetry: bool,
25}
26
27impl DshotCommand {
28    pub const STOP: u16 = 0;
29    pub const MIN_THROTTLE: u16 = 48;
30    pub const MAX_THROTTLE: u16 = 2047;
31    pub const FRAME_BITS: usize = 16;
32
33    pub const fn stop() -> Self {
34        Self {
35            throttle: Self::STOP,
36            telemetry: false,
37        }
38    }
39
40    pub fn from_normalized<R: FlightFloat>(value: R) -> Self {
41        let normalized = value.clamp(
42            <R as FlightFloat>::from_f32(0.0),
43            <R as FlightFloat>::from_f32(1.0),
44        );
45        let span = <R as FlightFloat>::from_u64((Self::MAX_THROTTLE - Self::MIN_THROTTLE) as u64);
46        Self {
47            throttle: (normalized * span + <R as FlightFloat>::from_u64(Self::MIN_THROTTLE as u64))
48                .to_f32_lossy() as u16,
49            telemetry: false,
50        }
51    }
52
53    pub fn frame(self) -> u16 {
54        let value = ((self.throttle & 0x07ff) << 1) | self.telemetry as u16;
55        let crc = (value ^ (value >> 4) ^ (value >> 8)) & 0x000f;
56        (value << 4) | crc
57    }
58
59    pub fn bit_is_high(frame: u16, bit_index: usize) -> bool {
60        let mask = 0x8000u16 >> bit_index.min(Self::FRAME_BITS - 1);
61        frame & mask != 0
62    }
63}
64
65pub fn output_protocol_for_rate<R: FlightFloat>(rate_hz: R) -> Result<PwmOutputProtocol, PwmError> {
66    if !rate_hz.is_finite() || rate_hz < <R as FlightFloat>::from_f32(0.0) {
67        return Err(PwmError::InvalidRate);
68    }
69
70    if rate_hz <= <R as FlightFloat>::from_f32(490.0) {
71        Ok(PwmOutputProtocol::StandardPwm)
72    } else if rate_hz >= <R as FlightFloat>::from_f32(150_000.0)
73        && rate_hz <= <R as FlightFloat>::from_f32(1_200_000.0)
74    {
75        Ok(PwmOutputProtocol::Dshot)
76    } else {
77        Err(PwmError::InvalidRate)
78    }
79}
80
81pub fn effective_output_rate_hz<R: FlightFloat>(rate_hz: R) -> Result<R, PwmError> {
82    let protocol = output_protocol_for_rate(rate_hz)?;
83    match protocol {
84        PwmOutputProtocol::StandardPwm if rate_hz == <R as FlightFloat>::from_f32(0.0) => {
85            Ok(<R as FlightFloat>::from_f32(STANDARD_PWM_DEFAULT_RATE_HZ))
86        }
87        _ => Ok(rate_hz),
88    }
89}
90
91pub trait PwmDriver<R: FlightFloat> {
92    fn len(&self) -> usize;
93    fn is_enabled(&self) -> bool;
94
95    fn enable(&mut self, channel: usize) -> Result<(), PwmError>;
96    fn disable(&mut self, channel: usize) -> Result<(), PwmError>;
97
98    fn enable_all(&mut self) -> Result<(), PwmError>;
99    fn disable_all(&mut self);
100
101    /// Sets the duty cycle for a specific channel.
102    ///
103    /// # Arguments
104    /// * `channel` - The output channel index (0-based).
105    /// * `duty`    - The desired duty cycle, typically represented as a u16 value.
106    ///             The exact interpretation (e.g., 0-ARR, 0-u16::MAX) depends
107    ///             on the implementation. For simulation, we'll map 0-u16::MAX
108    ///             to the simulator's expected range (e.g., 1000-2000us).
109    fn set_duty_cycle(&mut self, channel: usize, duty: u16) -> Result<(), PwmError>;
110
111    /// Sends the current state of all PWM channels to the output/simulator.
112    /// This should be called once per control loop after all individual
113    /// `set_duty_cycle` calls for that loop iteration are complete.
114    ///
115    /// # Arguments
116    /// * `now_us` - The current flight controller time in microseconds for timestamping.
117    fn flush<B: BoardIo>(&mut self, board: &mut B);
118
119    /// Configures default output update rates in Hz for mixer-owned channels.
120    ///
121    /// Boards that cannot change rates at runtime may ignore this hook, but
122    /// the core still propagates the mixer-owned rate resource explicitly.
123    fn configure_output_rates(&mut self, _rates_hz: &[R]) -> Result<(), PwmError> {
124        Ok(())
125    }
126
127    fn output_protocol(&self, _channel: usize) -> Result<PwmOutputProtocol, PwmError> {
128        Ok(PwmOutputProtocol::StandardPwm)
129    }
130
131    // actually loops over the channels (up to self.len()) and sends pwm commands via set_duty_cycle
132    fn send_commands<B: BoardIo>(&mut self, board: &mut B, commands: &[R]) -> Result<(), PwmError>;
133
134    fn send_disarmed_commands<B: BoardIo>(
135        &mut self,
136        board: &mut B,
137        output_types: &[MixerOutputType],
138    ) -> Result<(), PwmError> {
139        let mut commands =
140            [<R as FlightFloat>::from_f32(0.5); crate::pwm::output_sync::PWM_OUTPUT_CHANNELS];
141        for (channel, command) in commands.iter_mut().enumerate().take(self.len()) {
142            let output_type = output_types
143                .get(channel)
144                .copied()
145                .unwrap_or(MixerOutputType::Aux);
146            *command = safe_disarmed_command(output_type);
147        }
148        self.send_commands(board, &commands)
149    }
150}
151
152pub fn safe_disarmed_command<R: FlightFloat>(output_type: MixerOutputType) -> R {
153    match output_type {
154        MixerOutputType::Motor | MixerOutputType::Gpio => <R as FlightFloat>::from_f32(0.0),
155        MixerOutputType::Aux | MixerOutputType::Servo => <R as FlightFloat>::from_f32(0.5),
156    }
157}
158
159#[cfg(test)]
160mod tests {
161    use super::*;
162
163    #[test]
164    fn classifies_rosflight_pwm_and_dshot_rate_ranges() {
165        assert_eq!(
166            output_protocol_for_rate(50.0),
167            Ok(PwmOutputProtocol::StandardPwm)
168        );
169        assert_eq!(
170            output_protocol_for_rate(0.0),
171            Ok(PwmOutputProtocol::StandardPwm)
172        );
173        assert_eq!(
174            output_protocol_for_rate(490.0),
175            Ok(PwmOutputProtocol::StandardPwm)
176        );
177        assert_eq!(
178            output_protocol_for_rate(300_000.0),
179            Ok(PwmOutputProtocol::Dshot)
180        );
181        assert_eq!(
182            output_protocol_for_rate(10_000.0),
183            Err(PwmError::InvalidRate)
184        );
185    }
186
187    #[test]
188    fn zero_rate_uses_standard_pwm_default_rate() {
189        assert_eq!(effective_output_rate_hz(0.0), Ok(50.0));
190        assert_eq!(effective_output_rate_hz(490.0), Ok(490.0));
191        assert_eq!(effective_output_rate_hz(300_000.0), Ok(300_000.0));
192    }
193
194    #[test]
195    fn dshot_frame_matches_rosflight_checksum_formula() {
196        let command = DshotCommand {
197            throttle: 48,
198            telemetry: false,
199        };
200        let value = 48u16 << 1;
201        let expected_crc = (value ^ (value >> 4) ^ (value >> 8)) & 0x000f;
202        assert_eq!(command.frame(), (value << 4) | expected_crc);
203    }
204
205    #[test]
206    fn dshot_stop_frame_uses_zero_throttle() {
207        assert_eq!(DshotCommand::stop().throttle, 0);
208        assert_eq!(DshotCommand::stop().frame(), 0);
209    }
210}
veloxity_core/pwm.rs

veloxity_core/
pwm.rs
