kaptan/libs/kaptan-actuator/src/dshot.rs

//! DShot framing, timing, and arming policy.

use crate::ArmingDuration;
use core::sync::atomic::{AtomicBool, AtomicU16, Ordering};

pub const MIN_THROTTLE: u16 = 48;
pub const MOTOR_STOP: u16 = 0;
pub const FRAME_SYMBOLS: usize = 18;

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct Config {
    rate: Rate,
    armed_idle_throttle: Throttle,
    arming_duration: ArmingDuration,
}

impl Config {
    pub const fn new(
        rate: Rate,
        armed_idle_throttle: Throttle,
        arming_duration: ArmingDuration,
    ) -> Option<Self> {
        if rate.frames_for_millis(arming_duration.milliseconds()) == 0 {
            return None;
        }
        Some(Self {
            rate,
            armed_idle_throttle,
            arming_duration,
        })
    }

    pub const fn rate(self) -> Rate {
        self.rate
    }

    pub const fn armed_idle_throttle(self) -> Throttle {
        self.armed_idle_throttle
    }

    pub const fn arming_duration(self) -> ArmingDuration {
        self.arming_duration
    }
}

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum Rate {
    Dshot150,
    Dshot300,
    Dshot600,
}

impl Rate {
    pub const fn hertz(self) -> u32 {
        match self {
            Self::Dshot150 => 150_000,
            Self::Dshot300 => 300_000,
            Self::Dshot600 => 600_000,
        }
    }

    /// Twenty timer ticks per symbol produce exact 7/14 compare values.
    pub const fn timer_base_hz(self) -> u32 {
        self.hertz() * 20
    }

    pub const fn output_rate_hz(self) -> u32 {
        match self {
            Self::Dshot150 => 4_000,
            Self::Dshot300 | Self::Dshot600 => 8_000,
        }
    }

    pub const fn frames_for_millis(self, milliseconds: u16) -> u16 {
        ((self.output_rate_hz() * milliseconds as u32) / 1_000) as u16
    }
}

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct Timing {
    bit_ticks: u16,
    zero_high_ticks: u16,
    one_high_ticks: u16,
}

impl Timing {
    pub const fn from_timer_clock(timer_hz: u32, rate: Rate) -> Option<Self> {
        let symbol_hz = rate.hertz();
        let ticks = (timer_hz + symbol_hz / 2) / symbol_hz;
        if ticks < 3 || ticks > u16::MAX as u32 {
            return None;
        }
        let bit_ticks = ticks as u16;
        let zero_high_ticks = ((ticks * 7 + 10) / 20) as u16;
        let one_high_ticks = ((ticks * 14 + 10) / 20) as u16;
        if zero_high_ticks == 0 || one_high_ticks >= bit_ticks {
            return None;
        }
        Some(Self {
            bit_ticks,
            zero_high_ticks,
            one_high_ticks,
        })
    }

    pub const fn bit_ticks(self) -> u16 {
        self.bit_ticks
    }
    pub const fn zero_high_ticks(self) -> u16 {
        self.zero_high_ticks
    }
    pub const fn one_high_ticks(self) -> u16 {
        self.one_high_ticks
    }
}

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct Throttle(u16);

impl Throttle {
    pub const fn new(value: u16) -> Option<Self> {
        if value >= MIN_THROTTLE && value <= 2_047 {
            Some(Self(value))
        } else {
            None
        }
    }

    pub const fn get(self) -> u16 {
        self.0
    }
}

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum FrameValue {
    Stop,
    Throttle(Throttle),
}

impl FrameValue {
    pub const fn packet(self) -> u16 {
        packet(match self {
            Self::Stop => MOTOR_STOP,
            Self::Throttle(throttle) => throttle.get(),
        })
    }
}

pub struct ArmSequence {
    armed_throttle: AtomicU16,
    zero_frames: AtomicU16,
    remaining: AtomicU16,
    armed: AtomicBool,
}

impl ArmSequence {
    pub const fn new(armed_throttle: Throttle, zero_frames: u16) -> Self {
        Self {
            armed_throttle: AtomicU16::new(armed_throttle.get()),
            zero_frames: AtomicU16::new(zero_frames),
            remaining: AtomicU16::new(0),
            armed: AtomicBool::new(false),
        }
    }

    pub fn arm(&self) {
        self.remaining
            .store(self.zero_frames.load(Ordering::Acquire), Ordering::Release);
        self.armed.store(true, Ordering::Release);
    }

    pub fn configure_zero_frames(&self, zero_frames: u16) {
        self.zero_frames.store(zero_frames, Ordering::Release);
    }

    pub fn configure_armed_throttle(&self, armed_throttle: Throttle) {
        self.armed_throttle
            .store(armed_throttle.get(), Ordering::Release);
    }

    pub fn disarm(&self) {
        self.armed.store(false, Ordering::Release);
        self.remaining.store(0, Ordering::Release);
    }

    pub fn is_armed(&self) -> bool {
        self.armed.load(Ordering::Acquire)
    }

    pub fn next_frame(&self) -> Option<FrameValue> {
        if !self.is_armed() {
            return None;
        }
        if self.remaining.load(Ordering::Acquire) != 0 {
            self.remaining.fetch_sub(1, Ordering::AcqRel);
            return Some(FrameValue::Stop);
        }
        match Throttle::new(self.armed_throttle.load(Ordering::Acquire)) {
            Some(throttle) => Some(FrameValue::Throttle(throttle)),
            None => unreachable!(),
        }
    }
}

pub const fn packet(throttle: u16) -> u16 {
    let value = throttle << 1;
    (value << 4) | ((value ^ (value >> 4) ^ (value >> 8)) & 0x0f)
}

pub const fn symbols(packet: u16, zero_high: u16, one_high: u16) -> [u16; FRAME_SYMBOLS] {
    let mut symbols = [0; FRAME_SYMBOLS];
    let mut bit = 0;
    while bit < 16 {
        symbols[bit] = if packet & (1 << (15 - bit)) != 0 {
            one_high
        } else {
            zero_high
        };
        bit += 1;
    }
    symbols
}

#[cfg(test)]
mod tests {
    use super::{ArmSequence, Config, FrameValue, Rate, Throttle, Timing, packet, symbols};
    use crate::ArmingDuration;

    #[test]
    fn packet_matches_standard_dshot_vectors() {
        assert_eq!(packet(0), 0x0000);
        assert_eq!(packet(48), 0x0606);
        assert_eq!(packet(158), 0x13ce);
        assert_eq!(packet(300), 0x258f);
    }

    #[test]
    fn arm_sequence_holds_stop_for_the_configured_interval() {
        let throttle = Throttle::new(300).unwrap();
        let sequence = ArmSequence::new(throttle, 2);
        sequence.arm();
        assert_eq!(sequence.next_frame(), Some(FrameValue::Stop));
        assert_eq!(sequence.next_frame(), Some(FrameValue::Stop));
        assert_eq!(sequence.next_frame(), Some(FrameValue::Throttle(throttle)));
        sequence.disarm();
        assert_eq!(sequence.next_frame(), None);
    }

    #[test]
    fn symbols_end_with_the_required_low_reset_gap() {
        assert_eq!(symbols(packet(48), 7, 14)[16..], [0, 0]);
    }

    #[test]
    fn standard_rates_have_timer_profiles() {
        for rate in [Rate::Dshot150, Rate::Dshot300, Rate::Dshot600] {
            assert!(Timing::from_timer_clock(rate.timer_base_hz(), rate).is_some());
        }
    }

    #[test]
    fn requires_an_arming_interval_with_frames() {
        let duration = ArmingDuration::from_millis(2_000).unwrap();
        let throttle = Throttle::new(300).unwrap();
        assert!(Config::new(Rate::Dshot600, throttle, duration).is_some());
    }
}