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

//! Conventional PWM framing and arming policy.

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

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct Config {
    frame_rate: Rate,
    minimum_pulse: PulseWidth,
    armed_idle_pulse: PulseWidth,
    arming_duration: ArmingDuration,
}

impl Config {
    pub const fn new(
        frame_rate: Rate,
        minimum_pulse: PulseWidth,
        armed_idle_pulse: PulseWidth,
        arming_duration: ArmingDuration,
    ) -> Option<Self> {
        match frame_rate.period_microseconds() {
            Some(period)
                if minimum_pulse.microseconds() <= armed_idle_pulse.microseconds()
                    && armed_idle_pulse.microseconds() <= period =>
            {
                Some(Self {
                    frame_rate,
                    minimum_pulse,
                    armed_idle_pulse,
                    arming_duration,
                })
            }
            _ => None,
        }
    }

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

    pub const fn minimum_pulse(self) -> PulseWidth {
        self.minimum_pulse
    }

    pub const fn armed_idle_pulse(self) -> PulseWidth {
        self.armed_idle_pulse
    }

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

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

impl Rate {
    pub const fn new(hertz: u16) -> Option<Self> {
        if hertz != 0 && hertz <= 500 {
            Some(Self(hertz))
        } else {
            None
        }
    }

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

    pub const fn period_microseconds(self) -> Option<u16> {
        if 1_000_000 % self.0 as u32 == 0 {
            Some((1_000_000 / self.0 as u32) as u16)
        } else {
            None
        }
    }

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

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

impl PulseWidth {
    pub const fn new(microseconds: u16) -> Option<Self> {
        if microseconds >= 1_000 && microseconds <= 2_000 {
            Some(Self(microseconds))
        } else {
            None
        }
    }

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

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub enum FrameValue {
    Minimum,
    ArmedIdle(PulseWidth),
}

pub struct ArmSequence {
    armed_idle: AtomicU16,
    minimum_frames: AtomicU16,
    remaining: AtomicU16,
    armed: AtomicBool,
}

impl ArmSequence {
    pub const fn new(armed_idle: PulseWidth, minimum_frames: u16) -> Self {
        Self {
            armed_idle: AtomicU16::new(armed_idle.microseconds()),
            minimum_frames: AtomicU16::new(minimum_frames),
            remaining: AtomicU16::new(0),
            armed: AtomicBool::new(false),
        }
    }

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

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

    pub fn configure_armed_idle(&self, armed_idle: PulseWidth) {
        self.armed_idle
            .store(armed_idle.microseconds(), 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::Minimum);
        }
        match PulseWidth::new(self.armed_idle.load(Ordering::Acquire)) {
            Some(pulse) => Some(FrameValue::ArmedIdle(pulse)),
            None => unreachable!(),
        }
    }
}

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

    #[test]
    fn rate_describes_an_exact_400_hertz_frame() {
        let rate = Rate::new(400).unwrap();
        assert_eq!(rate.period_microseconds(), Some(2_500));
        assert_eq!(rate.frames_for_millis(2_000), 800);
    }

    #[test]
    fn sequence_holds_minimum_before_armed_idle() {
        let pulse = PulseWidth::new(1_100).unwrap();
        let sequence = ArmSequence::new(pulse, 2);
        sequence.arm();
        assert_eq!(sequence.next_frame(), Some(FrameValue::Minimum));
        assert_eq!(sequence.next_frame(), Some(FrameValue::Minimum));
        assert_eq!(sequence.next_frame(), Some(FrameValue::ArmedIdle(pulse)));
        sequence.disarm();
        assert_eq!(sequence.next_frame(), None);
    }

    #[test]
    fn requires_an_exact_frame_and_ordered_pulses() {
        let duration = ArmingDuration::from_millis(2_000).unwrap();
        let rate = Rate::new(400).unwrap();
        let minimum = PulseWidth::new(1_000).unwrap();
        let idle = PulseWidth::new(1_100).unwrap();
        assert!(Config::new(rate, minimum, idle, duration).is_some());
        assert!(Config::new(Rate::new(333).unwrap(), minimum, idle, duration).is_none());
        assert!(Config::new(rate, idle, minimum, duration).is_none());
    }
}