boards/kakute-f7-mini-v3: add selectable motor outputs
Signed-off-by: Onur Özkan <work@onurozkan.dev>
This commit is contained in:
Onur Özkan committed
1 parent
4a5d0313ca
commit
117e1abe97
12 files changed
+987
-88
No files matched your search
@@ -0,0 +1,84 @@
|
||||
mod tim3_dshot;
|
||||
mod tim3_pwm;
|
||||
|
||||
use core::sync::atomic::{AtomicU8, Ordering};
|
||||
|
||||
use kaptan_actuator::{MotorOutput, MotorProtocol};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
use self::{tim3_dshot::Tim3Dshot, tim3_pwm::Tim3Pwm};
|
||||
|
||||
const TRANSPORT_NONE: u8 = 0;
|
||||
const TRANSPORT_PWM: u8 = 1;
|
||||
const TRANSPORT_DSHOT: u8 = 2;
|
||||
|
||||
static ACTIVE_TRANSPORT: AtomicU8 = AtomicU8::new(TRANSPORT_NONE);
|
||||
|
||||
/// Kakute F7 Mini V3's protocol-neutral M1–M4 output route.
|
||||
pub(crate) struct KakuteMotorOutput {
|
||||
transport: Transport,
|
||||
}
|
||||
|
||||
enum Transport {
|
||||
Pwm(Tim3Pwm),
|
||||
Dshot(Tim3Dshot),
|
||||
}
|
||||
|
||||
impl KakuteMotorOutput {
|
||||
pub(crate) fn new(
|
||||
rcc: &stm32f722::RCC,
|
||||
tim3: stm32f722::TIM3,
|
||||
tim5: stm32f722::TIM5,
|
||||
protocol: MotorProtocol,
|
||||
) -> Self {
|
||||
let transport = match protocol {
|
||||
MotorProtocol::Pwm(config) => {
|
||||
ACTIVE_TRANSPORT.store(TRANSPORT_PWM, Ordering::Release);
|
||||
let _ = tim5;
|
||||
Transport::Pwm(Tim3Pwm::new(rcc, tim3, config))
|
||||
}
|
||||
MotorProtocol::Dshot(config) => {
|
||||
ACTIVE_TRANSPORT.store(TRANSPORT_DSHOT, Ordering::Release);
|
||||
Transport::Dshot(Tim3Dshot::new(rcc, tim3, tim5, config))
|
||||
}
|
||||
};
|
||||
Self { transport }
|
||||
}
|
||||
|
||||
/// Services the stable TIM3 interrupt route.
|
||||
pub(crate) fn service_tim3_interrupt() {
|
||||
if ACTIVE_TRANSPORT.load(Ordering::Acquire) == TRANSPORT_PWM {
|
||||
Tim3Pwm::on_tim3_update();
|
||||
}
|
||||
}
|
||||
|
||||
/// Services the stable TIM5 interrupt route.
|
||||
pub(crate) fn service_tim5_interrupt() {
|
||||
if ACTIVE_TRANSPORT.load(Ordering::Acquire) == TRANSPORT_DSHOT {
|
||||
Tim3Dshot::on_tim5_update();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorOutput for KakuteMotorOutput {
|
||||
fn arm(&mut self) {
|
||||
match &mut self.transport {
|
||||
Transport::Pwm(output) => output.arm(),
|
||||
Transport::Dshot(output) => output.arm(),
|
||||
}
|
||||
}
|
||||
|
||||
fn disarm(&mut self) {
|
||||
match &mut self.transport {
|
||||
Transport::Pwm(output) => output.disarm(),
|
||||
Transport::Dshot(output) => output.disarm(),
|
||||
}
|
||||
}
|
||||
|
||||
fn is_armed(&self) -> bool {
|
||||
match &self.transport {
|
||||
Transport::Pwm(output) => output.is_armed(),
|
||||
Transport::Dshot(output) => output.is_armed(),
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,403 @@
|
||||
use core::sync::atomic::{AtomicBool, Ordering};
|
||||
|
||||
use kaptan_actuator::{
|
||||
DshotConfig,
|
||||
dshot::{ArmSequence, FRAME_SYMBOLS, FrameValue, Throttle, Timing, symbols},
|
||||
};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
const APB1_TIMER_CLOCK_HZ: u32 = 108_000_000;
|
||||
const DSHOT_CHANNELS: usize = 4;
|
||||
const DSHOT_BUFFER_LENGTH: usize = FRAME_SYMBOLS * DSHOT_CHANNELS;
|
||||
const TIM3_CHANNEL_ENABLE: u32 = 0x1111;
|
||||
const PWM_MODE_1_WITH_PRELOAD: u32 = 0x6868;
|
||||
const TIM3_DMAR_CCR1_OFFSET: u8 = 13;
|
||||
const DEFAULT_ARMED_IDLE_THROTTLE: Throttle = match Throttle::new(48) {
|
||||
Some(throttle) => throttle,
|
||||
None => panic!("valid DShot throttle"),
|
||||
};
|
||||
|
||||
// These static buffers live in DMA-visible SRAM. They are initialized before
|
||||
// DMA is enabled and are subsequently read-only to the DMA controller.
|
||||
static mut DSHOT_ZERO_FRAME: [u32; DSHOT_BUFFER_LENGTH] = [0; DSHOT_BUFFER_LENGTH];
|
||||
static mut DSHOT_ARMED_IDLE_FRAME: [u32; DSHOT_BUFFER_LENGTH] = [0; DSHOT_BUFFER_LENGTH];
|
||||
static ARM_SEQUENCE: ArmSequence = ArmSequence::new(DEFAULT_ARMED_IDLE_THROTTLE, 0);
|
||||
static DMA_RUNNING: AtomicBool = AtomicBool::new(false);
|
||||
|
||||
pub(super) struct Tim3Dshot {
|
||||
timer: stm32f722::TIM3,
|
||||
dma_available: bool,
|
||||
}
|
||||
|
||||
impl Tim3Dshot {
|
||||
pub(super) fn new(
|
||||
rcc: &stm32f722::RCC,
|
||||
timer: stm32f722::TIM3,
|
||||
scheduler: stm32f722::TIM5,
|
||||
config: DshotConfig,
|
||||
) -> Self {
|
||||
let rate = config.rate();
|
||||
let dshot_timer_hz = rate.timer_base_hz();
|
||||
let timing = match Timing::from_timer_clock(dshot_timer_hz, rate) {
|
||||
Some(timing) => timing,
|
||||
None => unreachable!(),
|
||||
};
|
||||
let timer_prescaler = APB1_TIMER_CLOCK_HZ / dshot_timer_hz;
|
||||
assert!(APB1_TIMER_CLOCK_HZ.is_multiple_of(dshot_timer_hz));
|
||||
ARM_SEQUENCE
|
||||
.configure_zero_frames(rate.frames_for_millis(config.arming_duration().milliseconds()));
|
||||
ARM_SEQUENCE.configure_armed_throttle(config.armed_idle_throttle());
|
||||
|
||||
rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit());
|
||||
rcc.ahb1enr().modify(|_, write| write.dma1en().set_bit());
|
||||
rcc.apb1rstr().modify(|_, write| write.tim3rst().set_bit());
|
||||
rcc.apb1rstr()
|
||||
.modify(|_, write| write.tim3rst().clear_bit());
|
||||
rcc.apb1enr().modify(|_, write| write.tim3en().set_bit());
|
||||
|
||||
timer
|
||||
.cr1()
|
||||
.modify(|_, write| write.cen().clear_bit().arpe().enabled());
|
||||
timer.dier().modify(|_, write| write.ude().disabled());
|
||||
timer.ccer().write(|write| unsafe { write.bits(0) });
|
||||
timer
|
||||
.psc()
|
||||
.write(|write| unsafe { write.psc().bits((timer_prescaler - 1) as u16) });
|
||||
timer
|
||||
.arr()
|
||||
.write(|write| unsafe { write.bits(u32::from(timing.bit_ticks() - 1)) });
|
||||
timer
|
||||
.ccmr1_output()
|
||||
.write(|write| unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) });
|
||||
timer
|
||||
.ccmr2_output()
|
||||
.write(|write| unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) });
|
||||
timer.dcr().write(|write| unsafe {
|
||||
write
|
||||
.dba()
|
||||
.bits(TIM3_DMAR_CCR1_OFFSET)
|
||||
.dbl()
|
||||
.bits((DSHOT_CHANNELS - 1) as u8)
|
||||
});
|
||||
timer.egr().write(|write| write.ug().set_bit());
|
||||
let dma_available = disable_dma_stream();
|
||||
initialize_dma_frames(timing, config.armed_idle_throttle());
|
||||
|
||||
rcc.apb1rstr().modify(|_, write| write.tim5rst().set_bit());
|
||||
rcc.apb1rstr()
|
||||
.modify(|_, write| write.tim5rst().clear_bit());
|
||||
rcc.apb1enr().modify(|_, write| write.tim5en().set_bit());
|
||||
scheduler
|
||||
.psc()
|
||||
.write(|write| unsafe { write.psc().bits(0) });
|
||||
scheduler
|
||||
.arr()
|
||||
.write(|write| unsafe { write.bits(APB1_TIMER_CLOCK_HZ / rate.output_rate_hz() - 1) });
|
||||
scheduler.egr().write(|write| write.ug().set_bit());
|
||||
scheduler.sr().modify(|_, write| write.uif().clear());
|
||||
scheduler.dier().write(|write| write.uie().disabled());
|
||||
scheduler.cr1().write(|write| write.cen().clear_bit());
|
||||
configure_gpio_low();
|
||||
|
||||
Self {
|
||||
timer,
|
||||
dma_available,
|
||||
}
|
||||
}
|
||||
|
||||
pub(super) fn arm(&mut self) {
|
||||
if ARM_SEQUENCE.is_armed() {
|
||||
return;
|
||||
}
|
||||
if !self.dma_available {
|
||||
self.dma_available = disable_dma_stream();
|
||||
if !self.dma_available {
|
||||
return;
|
||||
}
|
||||
}
|
||||
ARM_SEQUENCE.arm();
|
||||
if !self.start_stream(zero_frame()) {
|
||||
ARM_SEQUENCE.disarm();
|
||||
return;
|
||||
}
|
||||
let scheduler = unsafe { &*stm32f722::TIM5::ptr() };
|
||||
scheduler.sr().modify(|_, write| write.uif().clear());
|
||||
scheduler.dier().modify(|_, write| write.uie().enabled());
|
||||
scheduler.cr1().modify(|_, write| write.cen().set_bit());
|
||||
}
|
||||
|
||||
pub(super) fn disarm(&mut self) {
|
||||
ARM_SEQUENCE.disarm();
|
||||
let scheduler = unsafe { &*stm32f722::TIM5::ptr() };
|
||||
scheduler.cr1().modify(|_, write| write.cen().clear_bit());
|
||||
scheduler.dier().modify(|_, write| write.uie().disabled());
|
||||
self.stop_stream();
|
||||
self.timer.ccer().write(|write| unsafe { write.bits(0) });
|
||||
configure_gpio_low();
|
||||
}
|
||||
|
||||
pub(super) fn is_armed(&self) -> bool {
|
||||
ARM_SEQUENCE.is_armed()
|
||||
}
|
||||
|
||||
fn start_stream(&mut self, frame: *const u32) -> bool {
|
||||
self.timer.cr1().modify(|_, write| write.cen().clear_bit());
|
||||
DMA_RUNNING.store(false, Ordering::Release);
|
||||
if !configure_dma_stream(frame) {
|
||||
return false;
|
||||
}
|
||||
self.timer
|
||||
.ccer()
|
||||
.write(|write| unsafe { write.bits(TIM3_CHANNEL_ENABLE) });
|
||||
configure_gpio_alternate_function();
|
||||
self.timer.egr().write(|write| write.ug().set_bit());
|
||||
DMA_RUNNING.store(true, Ordering::Release);
|
||||
dma_stream().cr().modify(|_, write| write.en().enabled());
|
||||
self.timer.dier().modify(|_, write| write.ude().enabled());
|
||||
self.timer.cr1().modify(|_, write| write.cen().set_bit());
|
||||
true
|
||||
}
|
||||
|
||||
fn stop_stream(&mut self) {
|
||||
self.timer.cr1().modify(|_, write| write.cen().clear_bit());
|
||||
DMA_RUNNING.store(false, Ordering::Release);
|
||||
self.timer.dier().modify(|_, write| write.ude().disabled());
|
||||
let _ = disable_dma_stream();
|
||||
}
|
||||
|
||||
pub(super) fn on_tim5_update() {
|
||||
let scheduler = unsafe { &*stm32f722::TIM5::ptr() };
|
||||
scheduler.sr().modify(|_, write| write.uif().clear());
|
||||
let timer = unsafe { &*stm32f722::TIM3::ptr() };
|
||||
timer.dier().modify(|_, write| write.ude().disabled());
|
||||
if !disable_dma_stream() {
|
||||
return;
|
||||
}
|
||||
if !DMA_RUNNING.load(Ordering::Acquire) {
|
||||
return;
|
||||
}
|
||||
let frame = match ARM_SEQUENCE.next_frame() {
|
||||
Some(FrameValue::Stop) => zero_frame(),
|
||||
Some(FrameValue::Throttle(_)) => armed_idle_frame(),
|
||||
None => return,
|
||||
};
|
||||
dma_stream()
|
||||
.ndtr()
|
||||
.write(|write| unsafe { write.bits(DSHOT_BUFFER_LENGTH as u32) });
|
||||
dma_stream()
|
||||
.m0ar()
|
||||
.write(|write| unsafe { write.bits(frame as u32) });
|
||||
dma_stream().cr().modify(|_, write| write.en().enabled());
|
||||
timer.dier().modify(|_, write| write.ude().enabled());
|
||||
}
|
||||
}
|
||||
|
||||
fn initialize_dma_frames(timing: Timing, armed_idle: Throttle) {
|
||||
write_dma_frame(
|
||||
core::ptr::addr_of_mut!(DSHOT_ZERO_FRAME).cast(),
|
||||
FrameValue::Stop,
|
||||
timing,
|
||||
);
|
||||
write_dma_frame(
|
||||
core::ptr::addr_of_mut!(DSHOT_ARMED_IDLE_FRAME).cast(),
|
||||
FrameValue::Throttle(armed_idle),
|
||||
timing,
|
||||
);
|
||||
}
|
||||
|
||||
fn write_dma_frame(destination: *mut u32, value: FrameValue, timing: Timing) {
|
||||
let symbols = symbols(
|
||||
value.packet(),
|
||||
timing.zero_high_ticks(),
|
||||
timing.one_high_ticks(),
|
||||
);
|
||||
let mut bit = 0;
|
||||
while bit < FRAME_SYMBOLS {
|
||||
let mut channel = 0;
|
||||
while channel < DSHOT_CHANNELS {
|
||||
unsafe {
|
||||
destination
|
||||
.add(bit * DSHOT_CHANNELS + channel)
|
||||
.write(symbols[bit] as u32)
|
||||
};
|
||||
channel += 1;
|
||||
}
|
||||
bit += 1;
|
||||
}
|
||||
}
|
||||
|
||||
fn zero_frame() -> *const u32 {
|
||||
core::ptr::addr_of!(DSHOT_ZERO_FRAME).cast()
|
||||
}
|
||||
fn armed_idle_frame() -> *const u32 {
|
||||
core::ptr::addr_of!(DSHOT_ARMED_IDLE_FRAME).cast()
|
||||
}
|
||||
|
||||
fn dma() -> &'static stm32f722::dma2::RegisterBlock {
|
||||
unsafe { &*stm32f722::DMA1::ptr() }
|
||||
}
|
||||
fn dma_stream() -> &'static stm32f722::dma2::ST {
|
||||
dma().st(2)
|
||||
}
|
||||
|
||||
fn disable_dma_stream() -> bool {
|
||||
dma_stream().cr().modify(|_, write| write.en().disabled());
|
||||
let mut spins = 1_024;
|
||||
while dma_stream().cr().read().en().is_enabled() && spins != 0 {
|
||||
spins -= 1;
|
||||
}
|
||||
if dma_stream().cr().read().en().is_enabled() {
|
||||
return false;
|
||||
}
|
||||
dma().lifcr().write(|write| {
|
||||
write
|
||||
.cfeif2()
|
||||
.clear()
|
||||
.cdmeif2()
|
||||
.clear()
|
||||
.cteif2()
|
||||
.clear()
|
||||
.chtif2()
|
||||
.clear()
|
||||
.ctcif2()
|
||||
.clear()
|
||||
});
|
||||
true
|
||||
}
|
||||
|
||||
fn configure_dma_stream(frame: *const u32) -> bool {
|
||||
if !disable_dma_stream() {
|
||||
return false;
|
||||
}
|
||||
dma_stream().cr().write(|write| unsafe {
|
||||
write
|
||||
.chsel()
|
||||
.bits(5)
|
||||
.dir()
|
||||
.memory_to_peripheral()
|
||||
.pl()
|
||||
.very_high()
|
||||
.minc()
|
||||
.incremented()
|
||||
.pinc()
|
||||
.fixed()
|
||||
.msize()
|
||||
.bits32()
|
||||
.psize()
|
||||
.bits32()
|
||||
.circ()
|
||||
.disabled()
|
||||
.tcie()
|
||||
.disabled()
|
||||
.teie()
|
||||
.disabled()
|
||||
.dmeie()
|
||||
.disabled()
|
||||
});
|
||||
dma_stream()
|
||||
.ndtr()
|
||||
.write(|write| unsafe { write.bits(DSHOT_BUFFER_LENGTH as u32) });
|
||||
dma_stream()
|
||||
.par()
|
||||
.write(|write| unsafe { write.bits(tim3_dmar_address()) });
|
||||
dma_stream()
|
||||
.m0ar()
|
||||
.write(|write| unsafe { write.bits(frame as u32) });
|
||||
dma_stream()
|
||||
.fcr()
|
||||
.write(|write| unsafe { write.fth().bits(3).dmdis().set_bit() });
|
||||
true
|
||||
}
|
||||
|
||||
fn tim3_dmar_address() -> u32 {
|
||||
unsafe { (&*stm32f722::TIM3::ptr()).dmar().as_ptr() as u32 }
|
||||
}
|
||||
|
||||
fn port() -> &'static stm32f722::gpiob::RegisterBlock {
|
||||
unsafe { &*stm32f722::GPIOB::ptr() }
|
||||
}
|
||||
|
||||
fn configure_gpio_low() {
|
||||
let port = port();
|
||||
port.moder().modify(|_, write| {
|
||||
write
|
||||
.moder0()
|
||||
.output()
|
||||
.moder1()
|
||||
.output()
|
||||
.moder4()
|
||||
.output()
|
||||
.moder5()
|
||||
.output()
|
||||
});
|
||||
port.otyper().modify(|_, write| {
|
||||
write
|
||||
.ot0()
|
||||
.push_pull()
|
||||
.ot1()
|
||||
.push_pull()
|
||||
.ot4()
|
||||
.push_pull()
|
||||
.ot5()
|
||||
.push_pull()
|
||||
});
|
||||
port.pupdr().modify(|_, write| {
|
||||
write
|
||||
.pupdr0()
|
||||
.floating()
|
||||
.pupdr1()
|
||||
.floating()
|
||||
.pupdr4()
|
||||
.floating()
|
||||
.pupdr5()
|
||||
.floating()
|
||||
});
|
||||
port.bsrr().write(|write| {
|
||||
write
|
||||
.br0()
|
||||
.set_bit()
|
||||
.br1()
|
||||
.set_bit()
|
||||
.br4()
|
||||
.set_bit()
|
||||
.br5()
|
||||
.set_bit()
|
||||
});
|
||||
}
|
||||
|
||||
fn configure_gpio_alternate_function() {
|
||||
let port = port();
|
||||
port.afrl().modify(|_, write| unsafe {
|
||||
write
|
||||
.afrl0()
|
||||
.bits(2)
|
||||
.afrl1()
|
||||
.bits(2)
|
||||
.afrl4()
|
||||
.bits(2)
|
||||
.afrl5()
|
||||
.bits(2)
|
||||
});
|
||||
port.moder().modify(|_, write| {
|
||||
write
|
||||
.moder0()
|
||||
.alternate()
|
||||
.moder1()
|
||||
.alternate()
|
||||
.moder4()
|
||||
.alternate()
|
||||
.moder5()
|
||||
.alternate()
|
||||
});
|
||||
port.ospeedr().modify(|_, write| {
|
||||
write
|
||||
.ospeedr0()
|
||||
.high_speed()
|
||||
.ospeedr1()
|
||||
.high_speed()
|
||||
.ospeedr4()
|
||||
.high_speed()
|
||||
.ospeedr5()
|
||||
.high_speed()
|
||||
});
|
||||
}
|
||||
@@ -0,0 +1,183 @@
|
||||
use core::sync::atomic::{AtomicU16, Ordering};
|
||||
|
||||
use kaptan_actuator::{
|
||||
PwmConfig,
|
||||
pwm::{ArmSequence, FrameValue, PulseWidth},
|
||||
};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
const APB1_TIMER_CLOCK_HZ: u32 = 108_000_000;
|
||||
const TIMER_TICK_HZ: u32 = 1_000_000;
|
||||
const PWM_CHANNEL_ENABLE: u32 = 0x1111;
|
||||
const PWM_MODE_1_WITH_PRELOAD: u32 = 0x6868;
|
||||
const DEFAULT_MINIMUM_PULSE: PulseWidth = match PulseWidth::new(1_000) {
|
||||
Some(pulse) => pulse,
|
||||
None => panic!("valid PWM pulse"),
|
||||
};
|
||||
const DEFAULT_ARMED_IDLE_PULSE: PulseWidth = match PulseWidth::new(1_000) {
|
||||
Some(pulse) => pulse,
|
||||
None => panic!("valid PWM pulse"),
|
||||
};
|
||||
|
||||
static ARM_SEQUENCE: ArmSequence = ArmSequence::new(DEFAULT_ARMED_IDLE_PULSE, 0);
|
||||
static MINIMUM_PULSE: AtomicU16 = AtomicU16::new(DEFAULT_MINIMUM_PULSE.microseconds());
|
||||
|
||||
pub(super) struct Tim3Pwm {
|
||||
timer: stm32f722::TIM3,
|
||||
minimum_pulse: PulseWidth,
|
||||
}
|
||||
|
||||
impl Tim3Pwm {
|
||||
pub(super) fn new(rcc: &stm32f722::RCC, timer: stm32f722::TIM3, config: PwmConfig) -> Self {
|
||||
let period_us = match config.frame_rate().period_microseconds() {
|
||||
Some(period) => period,
|
||||
None => unreachable!(),
|
||||
};
|
||||
ARM_SEQUENCE.configure_minimum_frames(
|
||||
config
|
||||
.frame_rate()
|
||||
.frames_for_millis(config.arming_duration().milliseconds()),
|
||||
);
|
||||
ARM_SEQUENCE.configure_armed_idle(config.armed_idle_pulse());
|
||||
MINIMUM_PULSE.store(config.minimum_pulse().microseconds(), Ordering::Release);
|
||||
|
||||
rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit());
|
||||
rcc.apb1rstr().modify(|_, write| write.tim3rst().set_bit());
|
||||
rcc.apb1rstr()
|
||||
.modify(|_, write| write.tim3rst().clear_bit());
|
||||
rcc.apb1enr().modify(|_, write| write.tim3en().set_bit());
|
||||
|
||||
configure_gpio_alternate_function();
|
||||
timer
|
||||
.cr1()
|
||||
.write(|write| write.cen().clear_bit().arpe().enabled());
|
||||
timer.psc().write(|write| unsafe {
|
||||
write
|
||||
.psc()
|
||||
.bits((APB1_TIMER_CLOCK_HZ / TIMER_TICK_HZ - 1) as u16)
|
||||
});
|
||||
timer
|
||||
.arr()
|
||||
.write(|write| unsafe { write.bits(u32::from(period_us - 1)) });
|
||||
timer
|
||||
.ccmr1_output()
|
||||
.write(|write| unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) });
|
||||
timer
|
||||
.ccmr2_output()
|
||||
.write(|write| unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) });
|
||||
write_pulse(&timer, config.minimum_pulse());
|
||||
timer
|
||||
.ccer()
|
||||
.write(|write| unsafe { write.bits(PWM_CHANNEL_ENABLE) });
|
||||
timer.egr().write(|write| write.ug().set_bit());
|
||||
timer.sr().modify(|_, write| write.uif().clear());
|
||||
timer.dier().write(|write| write.uie().enabled());
|
||||
timer.cr1().modify(|_, write| write.cen().set_bit());
|
||||
|
||||
Self {
|
||||
timer,
|
||||
minimum_pulse: config.minimum_pulse(),
|
||||
}
|
||||
}
|
||||
|
||||
pub(super) fn arm(&mut self) {
|
||||
ARM_SEQUENCE.arm();
|
||||
}
|
||||
|
||||
pub(super) fn disarm(&mut self) {
|
||||
ARM_SEQUENCE.disarm();
|
||||
write_pulse(&self.timer, self.minimum_pulse);
|
||||
}
|
||||
|
||||
pub(super) fn is_armed(&self) -> bool {
|
||||
ARM_SEQUENCE.is_armed()
|
||||
}
|
||||
|
||||
pub(super) fn on_tim3_update() {
|
||||
let timer = unsafe { &*stm32f722::TIM3::ptr() };
|
||||
timer.sr().modify(|_, write| write.uif().clear());
|
||||
let pulse = match ARM_SEQUENCE.next_frame() {
|
||||
Some(FrameValue::ArmedIdle(pulse)) => pulse,
|
||||
Some(FrameValue::Minimum) | None => configured_minimum_pulse(),
|
||||
};
|
||||
write_pulse(timer, pulse);
|
||||
}
|
||||
}
|
||||
|
||||
fn configured_minimum_pulse() -> PulseWidth {
|
||||
match PulseWidth::new(MINIMUM_PULSE.load(Ordering::Acquire)) {
|
||||
Some(pulse) => pulse,
|
||||
None => unreachable!(),
|
||||
}
|
||||
}
|
||||
|
||||
fn write_pulse(timer: &stm32f722::tim3::RegisterBlock, pulse: PulseWidth) {
|
||||
let ticks = u32::from(pulse.microseconds());
|
||||
timer.ccr1().write(|write| unsafe { write.bits(ticks) });
|
||||
timer.ccr2().write(|write| unsafe { write.bits(ticks) });
|
||||
timer.ccr3().write(|write| unsafe { write.bits(ticks) });
|
||||
timer.ccr4().write(|write| unsafe { write.bits(ticks) });
|
||||
}
|
||||
|
||||
fn port() -> &'static stm32f722::gpiob::RegisterBlock {
|
||||
unsafe { &*stm32f722::GPIOB::ptr() }
|
||||
}
|
||||
|
||||
fn configure_gpio_alternate_function() {
|
||||
let port = port();
|
||||
port.afrl().modify(|_, write| unsafe {
|
||||
write
|
||||
.afrl0()
|
||||
.bits(2)
|
||||
.afrl1()
|
||||
.bits(2)
|
||||
.afrl4()
|
||||
.bits(2)
|
||||
.afrl5()
|
||||
.bits(2)
|
||||
});
|
||||
port.moder().modify(|_, write| {
|
||||
write
|
||||
.moder0()
|
||||
.alternate()
|
||||
.moder1()
|
||||
.alternate()
|
||||
.moder4()
|
||||
.alternate()
|
||||
.moder5()
|
||||
.alternate()
|
||||
});
|
||||
port.otyper().modify(|_, write| {
|
||||
write
|
||||
.ot0()
|
||||
.push_pull()
|
||||
.ot1()
|
||||
.push_pull()
|
||||
.ot4()
|
||||
.push_pull()
|
||||
.ot5()
|
||||
.push_pull()
|
||||
});
|
||||
port.ospeedr().modify(|_, write| {
|
||||
write
|
||||
.ospeedr0()
|
||||
.high_speed()
|
||||
.ospeedr1()
|
||||
.high_speed()
|
||||
.ospeedr4()
|
||||
.high_speed()
|
||||
.ospeedr5()
|
||||
.high_speed()
|
||||
});
|
||||
port.pupdr().modify(|_, write| {
|
||||
write
|
||||
.pupdr0()
|
||||
.floating()
|
||||
.pupdr1()
|
||||
.floating()
|
||||
.pupdr4()
|
||||
.floating()
|
||||
.pupdr5()
|
||||
.floating()
|
||||
});
|
||||
}
|
||||
Reference in new issue
Block a user