chakokuのブログ(rev4)

日々のごった煮ブログです

STM32+RustでHALを使わず素でPWM制御

ドローン制御プログラムの続きで、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