diff --git a/Cargo.lock b/Cargo.lock index b1bfe03..19dc1d4 100644 --- a/Cargo.lock +++ b/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", diff --git a/Cargo.toml b/Cargo.toml index 4f6fa43..4bb4a42 100644 --- a/Cargo.toml +++ b/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", diff --git a/boards/kakute-f7-mini-v3/Cargo.toml b/boards/kakute-f7-mini-v3/Cargo.toml index 083497e..b4fbfed 100644 --- a/boards/kakute-f7-mini-v3/Cargo.toml +++ b/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" } diff --git a/libs/kaptan-actuator/Cargo.toml b/libs/kaptan-actuator/Cargo.toml new file mode 100644 index 0000000..283b41b --- /dev/null +++ b/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 diff --git a/libs/kaptan-actuator/src/dshot.rs b/libs/kaptan-actuator/src/dshot.rs new file mode 100644 index 0000000..87957fa --- /dev/null +++ b/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 { + 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 { + 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 { + 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()); + } + } +} diff --git a/libs/kaptan-actuator/src/lib.rs b/libs/kaptan-actuator/src/lib.rs new file mode 100644 index 0000000..0e2b7c1 --- /dev/null +++ b/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; +} diff --git a/libs/kaptan-actuator/src/protocol.rs b/libs/kaptan-actuator/src/protocol.rs new file mode 100644 index 0000000..51a358e --- /dev/null +++ b/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 { + 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 { + 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 { + 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()); + } +} diff --git a/libs/kaptan-actuator/src/pwm.rs b/libs/kaptan-actuator/src/pwm.rs new file mode 100644 index 0000000..d3d2b52 --- /dev/null +++ b/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 { + 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 { + 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 { + 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 { + 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); + } +}