ドローン制御プログラムの続きで、PWMを制御するためのドライバを開発。この理由は、ドローン用モータの制御はPWMで行うため。いきなりドローンでデバッグするのはモータが不用意に回って危険すぎるので、NUCLEO-L476RGのボードでテスト。 NUCELOのボードにはLEDが付いていて、PA5に接続されている。PA5のalternate functionとして、AF1を選択すると、TIM2_CH1が使える。以下は、TM2_CH1をPWMとして動かす場合のドライバ。実際のドローンは別のタイマーを使っていて、4CH実装する必要があるのでこのままでは使えない(ほとんど流用して作れると思っていますが)
最後のworkaroundは不細工な実装。レジスタ操作でカウンタを0に設定できれば、もっとキレイに実装できると思いますが・・
どれだけ見た人の参考になるか分かりませんが、、仕様を整理
- Timerに供給されるのは4MHzのクロック(自分の設定による)
- プリスケーラで100Hzまで分周
- ARR(Auto Reload Register)を10に設定することでカウンタが0->10(N=11?)を繰り返すので、PWM周期は10Hz(要確認だ・・・)
- CCR(Capture Compare Register)を0~10に変化させることで、dutyが0%~100%まで変化
(改めて書いていると、立ち木算のノリで、0を含めるかどうかで割り算が狂ってくるような。。)
STEVAL-DRONE01はセンサとの間がSPIで繋がっている。だから、SPI用ドライバも作る必要がある。しかも3-Wire方式の半二重だから余計に複雑なのであった。
(ドライバで半二重を実装するのは無理だったので、前回はソフトウエアでGPIOを操作してSPI相当のバスを作っていた(忘れてた)。素でSPIのレジスタが叩けたら、半二重に対応したSPIドライバが実装できるのではとも思うけど、、手間かかりそうだし。。まずは今のありものドライバを使って、傾き検知とモータ制御でドローンが浮上できるか試してみる)
// // stm32 device driver // // RCC pub const RCC_BASE: u32 = 0x4002_1000; pub const RCC_APB1ENR1_OFFSET: u32 = 0x58; pub const RCC_AHB2ENR_OFFSET : u32 = 0x4C; // RCC_AHB2ENR pub const RCC_CCIPR_OFFSET : u32 = 0x88; pub const RCC_I2C1SEL_FLD : u32 = 12; pub const RCC_USART2_SEL : u32 = 1; pub const RCC_GPIOAEN_BIT : u8 = 0; pub const RCC_GPIOBEN_BIT : u8 = 1; pub const RCC_GPIOCEN_BIT : u8 = 2; pub const RCC_GPIODEN_BIT : u8 = 3; pub const RCC_I2C1EN_BIT : u8 = 21; pub const RCC_USART2EN_BIT : u8 = 17; pub const RCC_TIM2EN_BIT : u8 = 0; // GPIO pub const GPIOA_BASE: u32 = 0x4800_0000; //APB2PERIPH_BASE pub const GPIOB_BASE: u32 = 0x4800_0400; pub const GPIOC_BASE: u32 = 0x4800_0800; pub const GPIOD_BASE: u32 = 0x4800_0C00; pub const GPIO_MODER_OFFSET: u32 = 0x0; // GPIOA pub const GPIO_OTYPER_OFFSET: u32 = 0x04; pub const GPIO_OSPEEDR_OFFSET: u32 = 0x08; pub const GPIO_PUPDR_OFFSET: u32 = 0x0C; pub const GPIO_BSRR_OFFSET: u32 = 0x18; pub const GPIO_AFRL_OFFSET: u32 = 0x20; pub const GPIO_AFRH_OFFSET: u32 = 0x24; pub const GPIO_PIN_1 : u8 = 1; pub const GPIO_PIN_2 : u8 = 2; pub const GPIO_PIN_3 : u8 = 3; pub const GPIO_PIN_4 : u8 = 4; pub const GPIO_PIN_5 : u8 = 5; pub const GPIO_PIN_8 : u8 = 8; pub const GPIO_PIN_9 : u8 = 9; //TIMER2 pub const TIM2_BASE : u32 = 0x4000_0000; // TIM2 pub const TIM_CE_BIT : u8 = 0; pub const TIM_CCMR_OFFSET : u32 = 0x18; pub const TIM_CR1_OFFSET : u32 = 0x00; pub const TIM_CCER_OFFSET : u32 = 0x20; pub const TIM_CNT_OFFSET : u32 = 0x24; pub const TIM_PSC_OFFSET : u32 = 0x28; pub const TIM_ARR_OFFSET : u32 = 0x2c; pub const TIM_CCR1_OFFSET : u32 = 0x34; pub const TIM_OCM1_BIT_FLD : u8 = 4; pub const TIM_OC1PE_BIT : u8 = 3; pub const TIM_ARPE_BIT : u8 = 7; pub const TIM_CC1E_BIT : u8 = 0; pub const ARR_MAX : u32 = 10; pub fn pwm_set_duty(duty:u8){ let mut ccr:u32 = 0; if duty == 0 { ccr = 0 } else if duty == 100 { ccr = ARR_MAX; }else{ ccr = ARR_MAX * (duty as u32) / 100 ; } // Set CCR let mut addr = (TIM2_BASE + TIM_CCR1_OFFSET) as *mut u32; // 0x34 unsafe { let current = core::ptr::read_volatile(addr) ; core::ptr::write_volatile(addr, ccr) ; } } pub fn pwm_setup(){ // set GPIO PA5 to alternate mode let mut addr = (GPIOA_BASE + GPIO_MODER_OFFSET) as *mut u32; let mut set_mode = 0x2 << (GPIO_PIN_5 * 2); // 0x2 ... Alternate MODE let mut mask:u32 = !(0x3 << (GPIO_PIN_5 * 2)); // mask unsafe { let current = core::ptr::read_volatile(addr) ; let mut val = current & mask; val |= set_mode; core::ptr::write_volatile(addr, val) ; } // set GPIO PA5 to AF1(TIM2_CH1) addr = (GPIOA_BASE + GPIO_AFRL_OFFSET) as *mut u32; set_mode = 0x1 << (GPIO_PIN_5 * 4); // AF1 ... TIM2_CH1 mask = !(0xF << (GPIO_PIN_5 * 4)); // mask unsafe { let current = core::ptr::read_volatile(addr) ; let mut val = current & mask; val |= set_mode; core::ptr::write_volatile(addr, val) ; } // set GPIO PA5 to push pull mode -> 0: push pull(reset value) addr = (GPIOA_BASE + GPIO_OTYPER_OFFSET) as *mut u32; mask = !(1 << GPIO_PIN_5); unsafe { let mut val = core::ptr::read_volatile(addr) ; val &= mask; core::ptr::write_volatile(addr, val) ; } // TIM2 Enable addr = (RCC_BASE + RCC_APB1ENR1_OFFSET) as *mut u32; // 0x58 set_mode = 0x1 << RCC_TIM2EN_BIT; // TIM2EN : 0 unsafe { let current = core::ptr::read_volatile(addr) ; let mut val = current | set_mode; core::ptr::write_volatile(addr, val); } //-------------------------------- // set Timer2 (TIM2) //-------------------------------- // enable Counter (CE) AND ARPE addr = (TIM2_BASE + TIM_CR1_OFFSET) as *mut u32; // 0 set_mode = 0x1 << TIM_CE_BIT; // UE_BIT = 0 set_mode |= 0x1 << TIM_ARPE_BIT; // UE_BIT = 7 unsafe { let current = core::ptr::read_volatile(addr) ; let val = current | set_mode; core::ptr::write_volatile(addr, val) ; } // Set Prescalar () addr = (TIM2_BASE + TIM_PSC_OFFSET) as *mut u32; // 0x28 let div = 4 * 1000 * 1000 / 100 - 1; // divied to 100Hz by prescalar unsafe { let current = core::ptr::read_volatile(addr) ; let val = div; core::ptr::write_volatile(addr, val) ; } // Set ARR () addr = (TIM2_BASE + TIM_ARR_OFFSET) as *mut u32; // 0x2c let auto_reload = ARR_MAX; // set value of AUTO-RELOAD to 10 unsafe { let current = core::ptr::read_volatile(addr) ; let val:u32 = auto_reload; core::ptr::write_volatile(addr, val) ; } // Set CCR addr = (TIM2_BASE + TIM_CCR1_OFFSET) as *mut u32; // 0x34 let cc_val = 9; // duty 20% unsafe { let current = core::ptr::read_volatile(addr) ; let val = cc_val; core::ptr::write_volatile(addr, val) ; } // set PWM MODE1 addr = (TIM2_BASE + TIM_CCMR_OFFSET) as *mut u32; // 0 set_mode = 0b110 << TIM_OCM1_BIT_FLD; // OCM1_BIT_FIELD = 4 set_mode |= 1 << TIM_OC1PE_BIT; // TIM_OC1PE = 3 mask = !(0x38); unsafe { let current = core::ptr::read_volatile(addr) ; let mut val = current & mask; val |= set_mode; core::ptr::write_volatile(addr, val) ; } // enable PWM addr = (TIM2_BASE + TIM_CCER_OFFSET) as *mut u32; // 0x20 set_mode = 1 << TIM_CC1E_BIT; // TIM_CC1E_BIT = 0 //set_mode = 3; unsafe { let current = core::ptr::read_volatile(addr) ; let val = current | set_mode; core::ptr::write_volatile(addr, val) ; } // // work around (set counter(TMS_CNT) to 0) // addr = (TIM2_BASE + TIM_CR1_OFFSET) as *mut u32; // 0 let mut current_val; unsafe{ current_val = core::ptr::read_volatile(addr) ; core::ptr::write_volatile(addr, 0) ; } addr = (TIM2_BASE + TIM_CNT_OFFSET) as *mut u32; // 0 unsafe{ core::ptr::write_volatile(addr, 0) ; } addr = (TIM2_BASE + TIM_CR1_OFFSET) as *mut u32; // 0 unsafe{ core::ptr::write_volatile(addr, current_val) ; } }
参考にした説明動画
Stm32 Timers in PWM mode - YouTube