Skip to content
Snippets Groups Projects
Commit 98c8c8d8 authored by Johannes Sjölund's avatar Johannes Sjölund
Browse files

Pwm control, fix examples

parent 59c7e987
No related branches found
No related tags found
No related merge requests found
......@@ -9,7 +9,7 @@ extern crate cortex_m;
extern crate cortex_m_rtfm as rtfm;
use cortex_m::peripheral::SystClkSource;
use f4::led::{self, LEDS};
use f4::led::{self, LED};
use rtfm::{app, Threshold};
// CONFIGURATION
......@@ -55,8 +55,8 @@ fn toggle(_t: &mut Threshold, r: SYS_TICK::Resources) {
**r.ON = !**r.ON;
if **r.ON {
LEDS[0].on();
LED.on();
} else {
LEDS[0].off();
LED.off();
}
}
......@@ -10,12 +10,11 @@ extern crate cortex_m_rtfm as rtfm;
extern crate f4;
use f4::Serial;
use f4::led::{self, LEDS};
use f4::led::{self, LED};
use f4::prelude::*;
use f4::serial::Event;
use f4::time::Hertz;
use cortex_m::peripheral::SystClkSource;
use cast::{usize, u8};
use rtfm::{app, Threshold};
// CONFIGURATION
......@@ -28,13 +27,13 @@ app! {
device: f4::stm32f40x,
resources: {
static STATE: u8 = 0;
static ON: bool = false;
},
tasks: {
SYS_TICK: {
path: roulette,
resources: [STATE],
resources: [ON],
},
USART2: {
......@@ -46,7 +45,7 @@ app! {
// INITIALIZATION PHASE
fn init(p: init::Peripherals, _r: init::Resources) {
led::init(p.GPIOB, p.RCC);
led::init(p.GPIOA, p.RCC);
let serial = Serial(p.USART2);
serial.init(BAUD_RATE.invert(), Some(p.DMA1), p.GPIOA, p.RCC);
......@@ -85,11 +84,11 @@ fn loopback(_t: &mut Threshold, r: USART2::Resources) {
}
fn roulette(_t: &mut Threshold, r: SYS_TICK::Resources) {
let curr = **r.STATE;
let next = (curr + 1) % u8(LEDS.len()).unwrap();
**r.ON = !**r.ON;
LEDS[usize(curr)].off();
LEDS[usize(next)].on();
**r.STATE = next;
if **r.ON {
LED.on();
} else {
LED.off();
}
}
......@@ -7,7 +7,7 @@
extern crate cortex_m_rtfm as rtfm;
extern crate f4;
use f4::led::{self, LEDS};
use f4::led::{self, LED};
use rtfm::app;
// TASKS & RESOURCES
......@@ -22,12 +22,10 @@ fn init(p: init::Peripherals) {
// IDLE LOOP
fn idle() -> ! {
for led in &LEDS {
led.on();
}
// Sleep
loop {
LED.on();
rtfm::wfi();
LED.off();
}
}
......@@ -21,7 +21,7 @@ extern crate heapless;
use cast::{usize, u8};
use cortex_m::peripheral::SystClkSource;
use f4::Serial;
use f4::led::{self, LEDS};
use f4::leds::{LEDS};
use f4::prelude::*;
use f4::serial::Event;
use heapless::Vec;
......@@ -59,7 +59,7 @@ app!{
// INITIALIZATION PHASE
fn init(p: init::Peripherals, _r: init::Resources) {
led::init(&p.GPIOA, &p.RCC);
f4::leds::init(&p.GPIOB, &p.RCC);
let serial = Serial(p.USART2);
serial.init(BAUD_RATE.invert(), Some(p.DMA1), p.GPIOA, p.RCC);
......
//! Output a PWM on pin PA0 and control its duty cycle via a serial interface
//!
//! - '*' increase duty by a factor of 2
//! - '+' increase duty by 1
//! - '-' decrease duty by 1
//! - '/' decrease duty by a factor of 2
#![deny(unsafe_code)]
#![deny(warnings)]
#![feature(proc_macro)]
#![no_std]
extern crate f4;
extern crate cortex_m_rtfm as rtfm;
use core::u32;
use f4::prelude::*;
use f4::time::Hertz;
use f4::{Channel, Pwm, Serial};
use f4::serial::Event;
use rtfm::{app, Threshold};
const BAUD_RATE: Hertz = Hertz(115_200);
const FREQUENCY: Hertz = Hertz(1_000);
app! {
device: f4::stm32f40x,
tasks: {
USART2: {
path: rx,
resources: [TIM4, USART2],
},
},
}
fn init(p: init::Peripherals) {
let pwm = Pwm(p.TIM4);
let serial = Serial(p.USART2);
serial.init(BAUD_RATE.invert(), None, p.GPIOA, p.RCC);
serial.listen(Event::Rxne);
pwm.init(FREQUENCY.invert(), None, p.GPIOA, p.GPIOB, p.GPIOC, p.RCC);
pwm.set_duty(Channel::_1, 0);
pwm.enable(Channel::_1);
}
fn idle() -> ! {
loop {
rtfm::wfi();
}
}
fn rx(_t: &mut Threshold, r: USART2::Resources) {
let pwm = Pwm(&**r.TIM4);
let serial = Serial(&**r.USART2);
let byte = serial.read().unwrap();
// Echo back to signal we are alive
serial.write(byte).unwrap();
match byte {
b'+' | b'-' | b'*' | b'/' => {
let duty = pwm.get_duty(Channel::_1);
match byte {
b'+' => {
let max = pwm.get_max_duty();
pwm.set_duty(Channel::_1, if duty < max { duty + 1 } else { max });
}
b'-' => {
pwm.set_duty(Channel::_1, duty.checked_sub(1).unwrap_or(0));
}
b'*' => {
let new_duty = duty.checked_mul(2).unwrap_or(u32::MAX);
let max_duty = pwm.get_max_duty();
if new_duty < max_duty {
pwm.set_duty(Channel::_1, new_duty)
}
}
b'/' => pwm.set_duty(Channel::_1, duty / 2),
_ => { /* unreachable */ }
}
}
_ => {}
}
}
\ No newline at end of file
......@@ -21,7 +21,7 @@ app! {
}
fn init(p: init::Peripherals) {
let pwm = Pwm(p.TIM3);
let pwm = Pwm(p.TIM1);
pwm.init(FREQUENCY.invert(), None, p.GPIOA,p.GPIOB,p.GPIOC, p.RCC);
let duty = pwm.get_max_duty() / 16;
......
......@@ -24,7 +24,7 @@ extern crate heapless;
use cast::{usize, u8};
use cortex_m::peripheral::SystClkSource;
use f4::Serial;
use f4::led::{self, LEDS};
use f4::leds::{LEDS};
use f4::prelude::*;
use f4::serial::Event;
use f4::time::Hertz;
......@@ -61,7 +61,7 @@ app! {
// INITIALIZATION PHASE
fn init(p: init::Peripherals, _r: init::Resources) {
led::init(&p.GPIOA, &p.RCC);
f4::leds::init(&p.GPIOB, &p.RCC);
let serial = Serial(p.USART2);
serial.init(BAUD_RATE.invert(), Some(p.DMA1), p.GPIOA, p.RCC);
......
......@@ -11,7 +11,7 @@ extern crate cortex_m_rtfm as rtfm;
use cast::{usize, u8};
use cortex_m::peripheral::SystClkSource;
use f4::led::{self, LEDS};
use f4::leds::{LEDS};
use rtfm::{app, Threshold};
// CONFIGURATION
......@@ -35,7 +35,7 @@ app! {
// INITIALIZATION PHASE
fn init(p: init::Peripherals, _r: init::Resources) {
led::init(p.GPIOB, p.RCC);
f4::leds::init(p.GPIOB, p.RCC);
p.SYST.set_clock_source(SystClkSource::Core);
p.SYST.set_reload(16_000_000 / DIVISOR);
......
......@@ -19,13 +19,18 @@ pub enum Error {
Transfer,
}
/// Channel 1 of DMA1
pub struct Dma1Channel1 {
_0: (),
}
/// Channel 2 of DMA1
pub struct Dma1Channel2 {
_0: (),
}
/// Channel 6 of DMA1
pub struct Dma1Channel6 {
/// Channel 4 of DMA1
pub struct Dma1Channel4 {
_0: (),
}
......@@ -34,6 +39,11 @@ pub struct Dma1Channel5 {
_0: (),
}
/// Channel 6 of DMA1
pub struct Dma1Channel6 {
_0: (),
}
/// Buffer to be used with a certain DMA `CHANNEL`
// NOTE(packed) workaround for rust-lang/rust#41315
#[repr(packed)]
......@@ -214,7 +224,7 @@ impl<T> Buffer<T, Dma1Channel2> {
}
}
impl<T> Buffer<T, Dma1Channel6> {
impl<T> Buffer<T, Dma1Channel4> {
/// Waits until the DMA releases this buffer
pub fn release(&self, dma1: &DMA1) -> nb::Result<(), Error> {
let state = self.state.get();
......@@ -223,12 +233,12 @@ impl<T> Buffer<T, Dma1Channel6> {
return Ok(());
}
if dma1.hisr.read().teif6().bit_is_set() {
if dma1.hisr.read().teif4().bit_is_set() {
Err(nb::Error::Other(Error::Transfer))
} else if dma1.hisr.read().tcif6().bit_is_set() {
} else if dma1.hisr.read().tcif4().bit_is_set() {
unsafe { self.unlock(state) }
dma1.hifcr.write(|w| w.ctcif6().set_bit());
dma1.s6cr.modify(|_, w| w.en().clear_bit());
dma1.hifcr.write(|w| w.ctcif4().set_bit());
dma1.s2cr.modify(|_, w| w.en().clear_bit());
Ok(())
} else {
Err(nb::Error::WouldBlock)
......@@ -250,10 +260,125 @@ impl<T> Buffer<T, Dma1Channel5> {
} else if dma1.hisr.read().tcif5().bit_is_set() {
unsafe { self.unlock(state) }
dma1.hifcr.write(|w| w.ctcif5().set_bit());
dma1.s5cr.modify(|_, w| w.en().clear_bit());
dma1.s2cr.modify(|_, w| w.en().clear_bit());
Ok(())
} else {
Err(nb::Error::WouldBlock)
}
}
}
impl<T> Buffer<T, Dma1Channel6> {
/// Waits until the DMA releases this buffer
pub fn release(&self, dma1: &DMA1) -> nb::Result<(), Error> {
let state = self.state.get();
if state == State::Unlocked {
return Ok(());
}
if dma1.hisr.read().teif6().bit_is_set() {
Err(nb::Error::Other(Error::Transfer))
} else if dma1.hisr.read().tcif6().bit_is_set() {
unsafe { self.unlock(state) }
dma1.hifcr.write(|w| w.ctcif6().set_bit());
dma1.s2cr.modify(|_, w| w.en().clear_bit());
Ok(())
} else {
Err(nb::Error::WouldBlock)
}
}
}
/// A circular buffer associated to a DMA `CHANNEL`
pub struct CircBuffer<B, CHANNEL> {
_marker: PhantomData<CHANNEL>,
buffer: UnsafeCell<[B; 2]>,
state: Cell<CircState>,
}
impl<B, CHANNEL> CircBuffer<B, CHANNEL> {
pub(crate) fn lock(&self) -> &[B; 2] {
assert_eq!(self.state.get(), CircState::Free);
self.state.set(CircState::MutatingFirstHalf);
unsafe { &*self.buffer.get() }
}
}
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
enum CircState {
/// Not in use by the DMA
Free,
/// The DMA is mutating the first half of the buffer
MutatingFirstHalf,
/// The DMA is mutating the second half of the buffer
MutatingSecondHalf,
}
impl<B> CircBuffer<B, Dma1Channel1> {
/// Constructs a circular buffer from two halves
pub const fn new(buffer: [B; 2]) -> Self {
CircBuffer {
_marker: PhantomData,
buffer: UnsafeCell::new(buffer),
state: Cell::new(CircState::Free),
}
}
/// Yields read access to the half of the circular buffer that's not
/// currently being mutated by the DMA
pub fn read<R, F>(&self, dma1: &DMA1, f: F) -> nb::Result<R, Error>
where
F: FnOnce(&B) -> R,
{
let state = self.state.get();
assert_ne!(state, CircState::Free);
let isr = dma1.lisr.read();
if isr.teif1().bit_is_set() {
Err(nb::Error::Other(Error::Transfer))
} else {
match state {
CircState::MutatingFirstHalf => if isr.tcif1().bit_is_set() {
Err(nb::Error::Other(Error::Overrun))
} else if isr.htif1().bit_is_set() {
dma1.lifcr.write(|w| w.chtif1().set_bit());
self.state.set(CircState::MutatingSecondHalf);
let ret = f(unsafe { &(*self.buffer.get())[0] });
if isr.tcif1().bit_is_set() {
Err(nb::Error::Other(Error::Overrun))
} else {
Ok(ret)
}
} else {
Err(nb::Error::WouldBlock)
},
CircState::MutatingSecondHalf => if isr.htif1().bit_is_set() {
Err(nb::Error::Other(Error::Overrun))
} else if isr.tcif1().bit_is_set() {
dma1.lifcr.write(|w| w.ctcif1().set_bit());
self.state.set(CircState::MutatingFirstHalf);
let ret = f(unsafe { &(*self.buffer.get())[1] });
if isr.htif1().bit_is_set() {
Err(nb::Error::Other(Error::Overrun))
} else {
Ok(ret)
}
} else {
Err(nb::Error::WouldBlock)
},
_ => unreachable!(),
}
}
}
}
\ No newline at end of file
//! Eight LEDs connected to PORTB
use stm32f40x::{GPIOB, RCC};
/// All the LEDs
pub static LEDS: [Led; 8] = [
Led { i: 2 },
Led { i: 1 },
Led { i: 15 },
Led { i: 14 },
Led { i: 13 },
Led { i: 5 },
Led { i: 4 },
Led { i: 10 },
];
/// An LED
pub struct Led {
i: u8,
}
impl Led {
/// Turns off the LED
pub fn off(&self) {
// NOTE(safe) atomic write
unsafe { (*GPIOB.get()).bsrr.write(|w| w.bits(1 << (self.i + 16))) }
}
/// Turns on the LED
pub fn on(&self) {
// NOTE(safe) atomic write
unsafe { (*GPIOB.get()).bsrr.write(|w| w.bits(1 << self.i)) }
}
}
/// Initializes all the user LEDs
pub fn init(gpioa: &GPIOB, rcc: &RCC) {
// Power up peripherals
rcc.ahb1enr.modify(|_, w| w.gpioben().set_bit());
// Configure pins 8-15 as outputs
gpioa
.moder
.modify(
|_, w| {unsafe {
w.moder2().bits(1)
.moder1().bits(1)
.moder15().bits(1)
.moder14().bits(1)
.moder13().bits(1)
.moder5().bits(1)
.moder4().bits(1)
.moder10().bits(1)
}
},
);
}
......@@ -16,7 +16,7 @@
//! [examples]: ./examples/index.html
#![deny(missing_docs)]
#![deny(warnings)]
// #![deny(warnings)]
#![feature(const_fn)]
#![feature(const_unsafe_cell_new)]
#![feature(const_cell_new)]
......@@ -37,6 +37,7 @@ pub mod examples;
pub mod dma;
pub mod led;
pub mod leds;
pub mod serial;
pub mod timer;
pub mod time;
......
......@@ -31,20 +31,181 @@
//! - CH4 = PB9
use core::any::{Any, TypeId};
// use core::marker::Unsize;
use core::marker::Unsize;
use cast::{u16, u32};
use hal;
// use static_ref::Static;
use static_ref::Static;
use stm32f40x::{DMA1, TIM1, TIM2, TIM3, TIM4, GPIOA, GPIOB, GPIOC, RCC};
// use dma::{self, Buffer, Dma1Channel2};
use dma::{self, Buffer, Dma1Channel2};
use timer::{Channel};
/// PWM driver
pub struct Pwm<'a, T>(pub &'a T)
where
T: 'a;
impl<'a> Pwm<'a, TIM1> {
/// Initializes the PWM module
pub fn init<P>(&self,
period: P,
dma1: Option<&DMA1>,
gpioa: &GPIOA,
gpiob: &GPIOB,
gpioc: &GPIOC,
rcc: &RCC
) where
P: Into<::apb2::Ticks>,
{
self._init(period.into(), dma1, gpioa, gpiob, gpioc, rcc)
}
fn _init(
&self,
period: ::apb2::Ticks,
dma1: Option<&DMA1>,
gpioa: &GPIOA,
gpiob: &GPIOB,
gpioc: &GPIOC,
rcc: &RCC,
) {
let tim1 = self.0;
rcc.apb2enr.modify(|_, w| w.tim1en().set_bit());
rcc.ahb1enr.modify(|_, w| w.gpioaen().set_bit());
// CH1 = PA8 = alternate push-pull
// CH2 = PA9 = alternate push-pull
// CH3 = PA10 = alternate push-pull
// CH4 = PA11 = alternate push-pull
gpioa.afrh.modify(|_, w| {
w.afrh8().bits(1)
.afrh9().bits(1)
.afrh10().bits(1)
.afrh11().bits(1)
});
gpioa.moder.modify(|_, w| {
w.moder8().bits(2)
.moder9().bits(2)
.moder10().bits(2)
.moder11().bits(2)
});
// PWM mode 1
tim1.ccmr1_output.modify(|_, w| unsafe{
w.oc1pe()
.set_bit()
.oc1m()
.bits(0b110)
.oc2pe()
.set_bit()
.oc2m()
.bits(0b110)
});
tim1.ccmr2_output.modify(|_, w| unsafe{
w.oc3pe()
.set_bit()
.oc3m()
.bits(0b110)
.oc4pe()
.set_bit()
.oc4m()
.bits(0b110)
});
tim1.ccer.modify(|_, w| {
w.cc1p()
.clear_bit()
.cc2p()
.clear_bit()
.cc3p()
.clear_bit()
.cc4p()
.clear_bit()
});
tim1.bdtr.modify(|_, w| w.moe().set_bit());
self._set_period(period);
tim1.cr1.write(|w| unsafe {
w.cms()
.bits(0b00)
.dir()
.bit(false)
.opm()
.bit(false)
.cen()
.set_bit()
});
}
fn _set_period(&self, period: ::apb2::Ticks) {
let period = period.0;
let psc = u16((period - 1) / (1 << 16)).unwrap();
self.0.psc.write(|w| unsafe{w.psc().bits(psc)});
let arr = u16(period / u32(psc + 1)).unwrap();
self.0.arr.write(|w| unsafe{w.arr().bits(arr)});
}
}
impl<'a> hal::Pwm for Pwm<'a, TIM1> {
type Channel = Channel;
type Time = ::apb2::Ticks;
type Duty = u16;
fn disable(&self, channel: Channel) {
match channel {
Channel::_1 => self.0.ccer.modify(|_, w| w.cc1e().clear_bit()),
Channel::_2 => self.0.ccer.modify(|_, w| w.cc2e().clear_bit()),
Channel::_3 => self.0.ccer.modify(|_, w| w.cc3e().clear_bit()),
Channel::_4 => self.0.ccer.modify(|_, w| w.cc4e().clear_bit()),
}
}
fn enable(&self, channel: Channel) {
match channel {
Channel::_1 => self.0.ccer.modify(|_, w| w.cc1e().set_bit()),
Channel::_2 => self.0.ccer.modify(|_, w| w.cc2e().set_bit()),
Channel::_3 => self.0.ccer.modify(|_, w| w.cc3e().set_bit()),
Channel::_4 => self.0.ccer.modify(|_, w| w.cc4e().set_bit()),
}
}
fn get_duty(&self, channel: Channel) -> u16 {
match channel {
Channel::_1 => self.0.ccr1.read().ccr1().bits(),
Channel::_2 => self.0.ccr2.read().ccr2().bits(),
Channel::_3 => self.0.ccr3.read().ccr3().bits(),
Channel::_4 => self.0.ccr4.read().ccr4().bits(),
}
}
fn get_max_duty(&self) -> u16 {
self.0.arr.read().arr().bits()
}
fn get_period(&self) -> ::apb2::Ticks {
::apb2::Ticks(u32(self.0.psc.read().bits() * self.0.arr.read().bits()))
}
fn set_duty(&self, channel: Channel, duty: u16) {
match channel {
Channel::_1 => self.0.ccr1.write(|w| unsafe{w.ccr1().bits(duty)}),
Channel::_2 => self.0.ccr2.write(|w| unsafe{w.ccr2().bits(duty)}),
Channel::_3 => self.0.ccr3.write(|w| unsafe{w.ccr3().bits(duty)}),
Channel::_4 => self.0.ccr4.write(|w| unsafe{w.ccr4().bits(duty)}),
}
}
fn set_period<P>(&self, period: P)
where
P: Into<::apb2::Ticks>,
{
self._set_period(period.into())
}
}
macro_rules! impl_Pwm {
($TIM:ident, $APB:ident) => {
......@@ -190,6 +351,7 @@ macro_rules! impl_Pwm {
// PWM mode 1
if tim.get_type_id() == TypeId::of::<TIM2>() {
// Ignore conflicting pin for channel 4
tim.ccmr1_output.modify(|_, w| unsafe {
w.oc1pe()
.set_bit()
......@@ -245,47 +407,45 @@ macro_rules! impl_Pwm {
self._set_period(period);
// if let Some(dma1) = dma1 {
// tim2.dier.modify(|_, w| w.ude().set_bit());
// if tim2.get_type_id() == TypeId::of::<TIM2>() {
// // TIM2_UP
// // mem2mem: Memory to memory mode disabled
// // pl: Medium priority
// // msize: Memory size = 8 bits
// // psize: Peripheral size = 16 bits
// // minc: Memory increment mode enabled
// // pinc: Peripheral increment mode disabled
// // circ: Circular mode disabled
// // dir: Transfer from memory to peripheral
// // tceie: Transfer complete interrupt enabled
// // en: Disabled
// dma1.ccr2.write(|w| unsafe {
// w.mem2mem()
// .clear_bit()
// .pl()
// .bits(0b01)
// .msize()
// .bits(0b00)
// .psize()
// .bits(0b01)
// .minc()
// .set_bit()
// .pinc()
// .clear_bit()
// .circ()
// .clear_bit()
// .dir()
// .set_bit()
// .tcie()
// .set_bit()
// .en()
// .clear_bit()
// });
// } else {
// unimplemented!()
// }
// }
if let Some(dma1) = dma1 {
tim.dier.modify(|_, w| w.ude().set_bit());
if tim.get_type_id() == TypeId::of::<TIM3>() {
// TIM3_CH4/UP
// mem2mem: Memory to memory mode disabled
// pl: Medium priority
// msize: Memory size = 8 bits
// psize: Peripheral size = 16 bits
// minc: Memory increment mode enabled
// pinc: Peripheral increment mode disabled
// circ: Circular mode disabled
// dir: Transfer from memory to peripheral
// tceie: Transfer complete interrupt enabled
// en: Disabled
dma1.s2cr.write(|w| unsafe {
w.pl()
.bits(0b01)
.msize()
.bits(0b00)
.psize()
.bits(0b01)
.minc()
.set_bit()
.pinc()
.clear_bit()
.circ()
.clear_bit()
.dir()
.bits(0)
.tcie()
.set_bit()
.en()
.clear_bit()
});
} else {
unimplemented!()
}
}
tim.cr1.write(|w| unsafe {
w.cms()
......@@ -309,45 +469,45 @@ macro_rules! impl_Pwm {
self.0.arr.write(|w| unsafe{w.bits(arr)});
}
// /// Uses `buffer` to continuously change the duty cycle on every period
// pub fn set_duties<B>(
// &self,
// dma1: &DMA1,
// channel: Channel,
// buffer: &Static<Buffer<B, Dma1Channel2>>,
// ) -> ::core::result::Result<(), dma::Error>
// where
// B: Unsize<[u8]>,
// {
// let tim2 = self.0;
// if tim2.get_type_id() == TypeId::of::<TIM2>() {
// if dma1.ccr2.read().en().bit_is_set() {
// return Err(dma::Error::InUse);
// }
// let buffer: &[u8] = buffer.lock();
// dma1.cndtr2
// .write(|w| unsafe { w.ndt().bits(u16(buffer.len()).unwrap()) });
// dma1.cpar2.write(|w| unsafe {
// match channel {
// Channel::_1 => w.bits(&tim2.ccr1 as *const _ as u32),
// Channel::_2 => w.bits(&tim2.ccr2 as *const _ as u32),
// Channel::_3 => w.bits(&tim2.ccr3 as *const _ as u32),
// Channel::_4 => w.bits(&tim2.ccr4 as *const _ as u32),
// }
// });
// dma1.cmar2
// .write(|w| unsafe { w.bits(buffer.as_ptr() as u32) });
// dma1.ccr2.modify(|_, w| w.en().set_bit());
// Ok(())
// } else {
// unimplemented!()
// }
// }
/// Uses `buffer` to continuously change the duty cycle on every period
pub fn set_duties<B>(
&self,
dma1: &DMA1,
channel: Channel,
buffer: &Static<Buffer<B, Dma1Channel2>>,
) -> ::core::result::Result<(), dma::Error>
where
B: Unsize<[u8]>,
{
let tim3 = self.0;
if tim3.get_type_id() == TypeId::of::<TIM3>() {
if dma1.s2cr.read().en().bit_is_set() {
return Err(dma::Error::InUse);
}
let buffer: &[u8] = buffer.lock();
dma1.s2ndtr
.write(|w| unsafe { w.ndt().bits(u16(buffer.len()).unwrap()) });
dma1.s2par.write(|w| unsafe {
match channel {
Channel::_1 => w.bits(&tim3.ccr1 as *const _ as u32),
Channel::_2 => w.bits(&tim3.ccr2 as *const _ as u32),
Channel::_3 => w.bits(&tim3.ccr3 as *const _ as u32),
Channel::_4 => w.bits(&tim3.ccr4 as *const _ as u32),
}
});
dma1.s2m0ar
.write(|w| unsafe { w.bits(buffer.as_ptr() as u32) });
dma1.s2cr.modify(|_, w| w.en().set_bit());
Ok(())
} else {
unimplemented!()
}
}
}
impl<'a> hal::Pwm for Pwm<'a, $TIM>
......@@ -357,21 +517,12 @@ macro_rules! impl_Pwm {
type Time = ::$APB::Ticks;
fn get_duty(&self, channel: Channel) -> u32 {
// if self.0.get_type_id() == TypeId::of::<TIM1>() {
// match channel {
// Channel::_1 => u32(u32(self.0.ccr1.read().bits()),
// Channel::_2 => u32(u32(self.0.ccr2.read().bits()),
// Channel::_3 => u32(u32(self.0.ccr3.read().bits()),
// Channel::_4 => u32(u32(self.0.ccr4.read().bits()),
// }
// } else {
match channel {
Channel::_1 => u32(self.0.ccr1.read().ccr1_h().bits()) << 16 | u32(self.0.ccr1.read().ccr1_l().bits()),
Channel::_2 => u32(self.0.ccr2.read().ccr2_h().bits()) << 16 | u32(self.0.ccr2.read().ccr2_l().bits()),
Channel::_3 => u32(self.0.ccr3.read().ccr3_h().bits()) << 16 | u32(self.0.ccr3.read().ccr3_l().bits()),
Channel::_4 => u32(self.0.ccr4.read().ccr4_h().bits()) << 16 | u32(self.0.ccr4.read().ccr4_l().bits()),
}
// }
}
fn disable(&self, channel: Channel) {
......@@ -421,7 +572,6 @@ macro_rules! impl_Pwm {
}
}
// impl_Pwm!(TIM1,apb2);
impl_Pwm!(TIM2,apb1);
impl_Pwm!(TIM3,apb1);
impl_Pwm!(TIM4,apb1);
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment