libs/kaptan-actuator: add protocol-neutral motor policy

Signed-off-by: Onur Özkan <work@onurozkan.dev>

libs/kaptan-actuator/src/pwm.rs

@@ -0,0 +1,137 @@
+//! Conventional PWM framing and arming policy.
+
+use core::sync::atomic::{AtomicBool, AtomicU16, Ordering};
+
+#[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, FrameValue, PulseWidth, Rate};
+
+    #[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);
+    }
+}