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

//! Conventional PWM framing and arming policy.

use crate::ArmingDuration;

#[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: PulseWidth,
    minimum_frames: u16,
    remaining: u16,
    armed: bool,
}

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

    /// Starts the fixed configured minimum-frame sequence. The driver which
    /// owns this state must acquire external safety authorization before
    /// calling this method and before every subsequent non-minimum commit.
    pub fn arm(&mut self) {
        self.remaining = self.minimum_frames;
        self.armed = true;
    }

    /// Stops the sequence; the physical output owner must drive its explicit
    /// minimum pulse independently of this local state transition.
    pub fn disarm(&mut self) {
        self.armed = false;
        self.remaining = 0;
    }

    pub const fn is_armed(&self) -> bool {
        self.armed
    }

    pub fn next_frame(&mut self) -> Option<FrameValue> {
        if !self.is_armed() {
            return None;
        }
        if self.remaining != 0 {
            self.remaining -= 1;
            return Some(FrameValue::Minimum);
        }
        Some(FrameValue::ArmedIdle(self.armed_idle))
    }
}

#[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 mut 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());
    }
}