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

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

Cargo.lock

@@ -135,6 +135,10 @@ dependencies = [
  "hashbrown",
 ]
 
+[[package]]
+name = "kaptan-actuator"
+version = "0.1.0"
+
 [[package]]
 name = "kaptan-dma"
 version = "0.1.0"
@@ -154,6 +158,7 @@ version = "0.1.0"
 dependencies = [
  "cortex-m",
  "cortex-m-rt",
+ "kaptan-actuator",
  "kaptan-dma",
  "kaptan-drivers",
  "kaptan-mavlink",

Cargo.toml

@@ -1,6 +1,7 @@
 [workspace]
 members = [
     "boards/kakute-f7-mini-v3",
+    "libs/kaptan-actuator",
     "libs/kaptan-dma",
     "libs/kaptan-drivers",
     "libs/kaptan-mavlink",

boards/kakute-f7-mini-v3/Cargo.toml

@@ -15,6 +15,7 @@ firmware = []
 [dependencies]
 cortex-m = "0.7"
 cortex-m-rt = "0.7"
+kaptan-actuator = { path = "../../libs/kaptan-actuator" }
 kaptan-drivers = { path = "../../libs/kaptan-drivers" }
 kaptan-dma = { path = "../../libs/kaptan-dma" }
 kaptan-mavlink = { path = "../../libs/kaptan-mavlink" }

libs/kaptan-actuator/Cargo.toml

@@ -0,0 +1,10 @@
+[package]
+name = "kaptan-actuator"
+edition.workspace = true
+license.workspace = true
+repository.workspace = true
+rust-version.workspace = true
+version.workspace = true
+
+[lints]
+workspace = true

libs/kaptan-actuator/src/dshot.rs

@@ -0,0 +1,222 @@
+//! DShot framing, timing, and arming policy.
+
+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 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, FrameValue, Rate, Throttle, Timing, packet, symbols};
+
+    #[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());
+        }
+    }
+}

libs/kaptan-actuator/src/lib.rs

@@ -0,0 +1,19 @@
+//! Protocol-neutral motor-output policy.
+
+#![no_std]
+
+pub mod dshot;
+mod protocol;
+pub mod pwm;
+
+pub use protocol::{ArmingDuration, DshotConfig, MotorProtocol, PwmConfig};
+
+/// Flight-control and MAVLink boundary for motor outputs.
+pub trait MotorOutput {
+    /// Enables the configured armed output sequence.
+    fn arm(&mut self);
+    /// Stops output and returns all motor channels to their safe state.
+    fn disarm(&mut self);
+    /// Returns whether the output has accepted an arm request.
+    fn is_armed(&self) -> bool;
+}

libs/kaptan-actuator/src/protocol.rs

@@ -0,0 +1,132 @@
+use crate::{dshot, pwm};
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct ArmingDuration(u16);
+
+impl ArmingDuration {
+    pub const fn from_millis(milliseconds: u16) -> Option<Self> {
+        if milliseconds != 0 && milliseconds <= 8_000 {
+            Some(Self(milliseconds))
+        } else {
+            None
+        }
+    }
+
+    pub const fn milliseconds(self) -> u16 {
+        self.0
+    }
+}
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct PwmConfig {
+    frame_rate: pwm::Rate,
+    minimum_pulse: pwm::PulseWidth,
+    armed_idle_pulse: pwm::PulseWidth,
+    arming_duration: ArmingDuration,
+}
+
+impl PwmConfig {
+    pub const fn new(
+        frame_rate: pwm::Rate,
+        minimum_pulse: pwm::PulseWidth,
+        armed_idle_pulse: pwm::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) -> pwm::Rate {
+        self.frame_rate
+    }
+    pub const fn minimum_pulse(self) -> pwm::PulseWidth {
+        self.minimum_pulse
+    }
+    pub const fn armed_idle_pulse(self) -> pwm::PulseWidth {
+        self.armed_idle_pulse
+    }
+    pub const fn arming_duration(self) -> ArmingDuration {
+        self.arming_duration
+    }
+}
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub struct DshotConfig {
+    rate: dshot::Rate,
+    armed_idle_throttle: dshot::Throttle,
+    arming_duration: ArmingDuration,
+}
+
+impl DshotConfig {
+    pub const fn new(
+        rate: dshot::Rate,
+        armed_idle_throttle: dshot::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) -> dshot::Rate {
+        self.rate
+    }
+    pub const fn armed_idle_throttle(self) -> dshot::Throttle {
+        self.armed_idle_throttle
+    }
+    pub const fn arming_duration(self) -> ArmingDuration {
+        self.arming_duration
+    }
+}
+
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum MotorProtocol {
+    Pwm(PwmConfig),
+    Dshot(DshotConfig),
+}
+
+#[cfg(test)]
+mod tests {
+    use super::{ArmingDuration, DshotConfig, MotorProtocol, PwmConfig};
+    use crate::{dshot, pwm};
+
+    const ARMING: ArmingDuration = match ArmingDuration::from_millis(2_000) {
+        Some(duration) => duration,
+        None => panic!("test duration must be valid"),
+    };
+
+    #[test]
+    fn pwm_configuration_requires_an_exact_frame_and_ordered_pulses() {
+        let rate = pwm::Rate::new(400).unwrap();
+        let minimum = pwm::PulseWidth::new(1_000).unwrap();
+        let idle = pwm::PulseWidth::new(1_100).unwrap();
+        assert!(PwmConfig::new(rate, minimum, idle, ARMING).is_some());
+        assert!(PwmConfig::new(pwm::Rate::new(333).unwrap(), minimum, idle, ARMING).is_none());
+        assert!(PwmConfig::new(rate, idle, minimum, ARMING).is_none());
+    }
+
+    #[test]
+    fn motor_protocol_contains_only_validated_policies() {
+        let idle = dshot::Throttle::new(300).unwrap();
+        let config = DshotConfig::new(dshot::Rate::Dshot600, idle, ARMING).unwrap();
+        assert_eq!(MotorProtocol::Dshot(config), MotorProtocol::Dshot(config));
+        assert!(ArmingDuration::from_millis(0).is_none());
+    }
+}

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);
+    }
+}