Advertisement
Guest User

actuators_pwm_arch.c

a guest
Jun 20th, 2014
225
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 7.99 KB | None
  1. /*
  2.  * Copyright (C) 2010 The Paparazzi Team
  3.  *
  4.  * This file is part of Paparazzi.
  5.  *
  6.  * Paparazzi is free software; you can redistribute it and/or modify
  7.  * it under the terms of the GNU General Public License as published by
  8.  * the Free Software Foundation; either version 2, or (at your option)
  9.  * any later version.
  10.  *
  11.  * Paparazzi is distributed in the hope that it will be useful,
  12.  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13.  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14.  * GNU General Public License for more details.
  15.  *
  16.  * You should have received a copy of the GNU General Public License
  17.  * along with Paparazzi; see the file COPYING.  If not, write to
  18.  * the Free Software Foundation, 59 Temple Place - Suite 330,
  19.  * Boston, MA 02111-1307, USA.
  20.  */
  21.  
  22. /** @file arch/stm32/subsystems/actuators/actuators_pwm_arch.c
  23.  *  STM32 PWM servos handling.
  24.  */
  25.  
  26. #include "subsystems/actuators/actuators_pwm_arch.h"
  27. #include "subsystems/actuators/actuators_pwm.h"
  28.  
  29. #include <libopencm3/stm32/gpio.h>
  30. #include <libopencm3/stm32/rcc.h>
  31. #include <libopencm3/stm32/timer.h>
  32.  
  33. int32_t actuators_pwm_values[ACTUATORS_PWM_NB];
  34.  
  35. #if defined(STM32F1)
  36. //#define PCLK 72000000
  37. #define PCLK AHB_CLK
  38. #elif defined(STM32F4)
  39. //#define PCLK 84000000
  40. #define PCLK AHB_CLK/2
  41. #endif
  42.  
  43. #define ONE_MHZ_CLK 1000000
  44. /** Default servo update rate in Hz */
  45. #ifndef SERVO_HZ
  46. #define SERVO_HZ 40
  47. #endif
  48.  
  49. // Update rate can be adapted for each timer
  50. #ifndef TIM1_SERVO_HZ
  51. #define TIM1_SERVO_HZ SERVO_HZ
  52. #endif
  53. #ifndef TIM2_SERVO_HZ
  54. #define TIM2_SERVO_HZ SERVO_HZ
  55. #endif
  56. #ifndef TIM3_SERVO_HZ
  57. #define TIM3_SERVO_HZ SERVO_HZ
  58. #endif
  59. #ifndef TIM4_SERVO_HZ
  60. #define TIM4_SERVO_HZ SERVO_HZ
  61. #endif
  62. #ifndef TIM5_SERVO_HZ
  63. #define TIM5_SERVO_HZ SERVO_HZ
  64. #endif
  65.  
  66. /** Set PWM channel configuration
  67.  */
  68. static inline void actuators_pwm_arch_channel_init(u32 timer_peripheral,
  69.                         enum tim_oc_id oc_id) {
  70.  
  71.   timer_disable_oc_output(timer_peripheral, oc_id);
  72.   timer_disable_oc_clear(timer_peripheral, oc_id);
  73.   timer_enable_oc_preload(timer_peripheral, oc_id);
  74.   timer_set_oc_slow_mode(timer_peripheral, oc_id);
  75.   timer_set_oc_mode(timer_peripheral, oc_id, TIM_OCM_PWM1);
  76.   timer_set_oc_polarity_high(timer_peripheral, oc_id);
  77.   timer_enable_oc_output(timer_peripheral, oc_id);
  78. }
  79.  
  80. /** Set GPIO configuration
  81.  */
  82. #if defined(STM32F4)
  83. static inline void set_servo_gpio(u32 gpioport, u16 pin, u8 af_num, u32 en) {
  84.   rcc_peripheral_enable_clock(&RCC_AHB1ENR, en);
  85.   gpio_mode_setup(gpioport, GPIO_MODE_AF, GPIO_PUPD_NONE, pin);
  86.   gpio_set_af(gpioport, af_num, pin);
  87. }
  88. #elif defined(STM32F1)
  89. static inline void set_servo_gpio(u32 gpioport, u16 pin, u8 none, u32 en) {
  90.   rcc_peripheral_enable_clock(&RCC_APB2ENR, en | RCC_APB2ENR_AFIOEN);
  91.   gpio_set_mode(gpioport, GPIO_MODE_OUTPUT_50_MHZ,
  92.                 GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, pin);
  93. }
  94. #endif
  95.  
  96. /** Set Timer configuration
  97.  */
  98. static inline void set_servo_timer(u32 timer, u32 period, u8 channels_mask) {
  99.   timer_reset(timer);
  100.  
  101.   /* Timer global mode:
  102.    * - No divider.
  103.    * - Alignement edge.
  104.    * - Direction up.
  105.    */
  106.   timer_set_mode(timer, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
  107.  
  108.   timer_set_prescaler(timer, (PCLK / ONE_MHZ_CLK) - 1); // 1uS
  109.  
  110.   timer_disable_preload(timer);
  111.  
  112.   timer_continuous_mode(timer);
  113.  
  114.   timer_set_period(timer, (ONE_MHZ_CLK / period) - 1);
  115.  
  116.   /* Disable outputs and configure channel if needed. */
  117.   if (bit_is_set(channels_mask, 0)) {
  118.     actuators_pwm_arch_channel_init(timer, TIM_OC1);
  119.   }
  120.   if (bit_is_set(channels_mask, 1)) {
  121.     actuators_pwm_arch_channel_init(timer, TIM_OC2);
  122.   }
  123.   if (bit_is_set(channels_mask, 2)) {
  124.     actuators_pwm_arch_channel_init(timer, TIM_OC3);
  125.   }
  126.   if (bit_is_set(channels_mask, 3)) {
  127.     actuators_pwm_arch_channel_init(timer, TIM_OC4);
  128.   }
  129.  
  130.   /*
  131.    * Set initial output compare values.
  132.    * Note: Maybe we should preload the compare registers with some sensible
  133.    * values before we enable the timer?
  134.    */
  135.   //timer_set_oc_value(timer, TIM_OC1, 1000);
  136.   //timer_set_oc_value(timer, TIM_OC2, 1000);
  137.   //timer_set_oc_value(timer, TIM_OC3, 1000);
  138.   //timer_set_oc_value(timer, TIM_OC4, 1000);
  139.  
  140.   /* -- Enable timer -- */
  141.   /*
  142.    * ARR reload enable.
  143.    * Note: In our case it does not matter much if we do preload or not. As it
  144.    * is unlikely we will want to change the frequency of the timer during
  145.    * runtime anyways.
  146.    */
  147.   timer_enable_preload(timer);
  148.  
  149.   /* Counter enable. */
  150.   timer_enable_counter(timer);
  151. }
  152.  
  153. /** PWM arch init called by generic pwm driver
  154.  */
  155. void actuators_pwm_arch_init(void) {
  156.  
  157.   /*-----------------------------------
  158.    * Configure timer peripheral clocks
  159.    *-----------------------------------*/
  160. #if PWM_USE_TIM1
  161.   rcc_peripheral_enable_clock(&RCC_APB2ENR, RCC_APB2ENR_TIM1EN);
  162. #endif
  163. #if PWM_USE_TIM2
  164.   rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM2EN);
  165. #endif
  166. #if PWM_USE_TIM3
  167.   rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM3EN);
  168. #endif
  169. #if PWM_USE_TIM4
  170.   rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM4EN);
  171. #endif
  172. #if PWM_USE_TIM5
  173.   rcc_peripheral_enable_clock(&RCC_APB1ENR, RCC_APB1ENR_TIM5EN);
  174. #endif
  175.  
  176.   /*----------------
  177.    * Configure GPIO
  178.    *----------------*/
  179.  
  180. #ifdef PWM_SERVO_0
  181.   set_servo_gpio(PWM_SERVO_0_GPIO, PWM_SERVO_0_PIN, PWM_SERVO_0_AF, PWM_SERVO_0_RCC_IOP);
  182. #endif
  183. #ifdef PWM_SERVO_1
  184.   set_servo_gpio(PWM_SERVO_1_GPIO, PWM_SERVO_1_PIN, PWM_SERVO_1_AF, PWM_SERVO_1_RCC_IOP);
  185. #endif
  186. #ifdef PWM_SERVO_2
  187.   set_servo_gpio(PWM_SERVO_2_GPIO, PWM_SERVO_2_PIN, PWM_SERVO_2_AF, PWM_SERVO_2_RCC_IOP);
  188. #endif
  189. #ifdef PWM_SERVO_3
  190.   set_servo_gpio(PWM_SERVO_3_GPIO, PWM_SERVO_3_PIN, PWM_SERVO_3_AF, PWM_SERVO_3_RCC_IOP);
  191. #endif
  192. #ifdef PWM_SERVO_4
  193.   set_servo_gpio(PWM_SERVO_4_GPIO, PWM_SERVO_4_PIN, PWM_SERVO_4_AF, PWM_SERVO_4_RCC_IOP);
  194. #endif
  195. #ifdef PWM_SERVO_5
  196.   set_servo_gpio(PWM_SERVO_5_GPIO, PWM_SERVO_5_PIN, PWM_SERVO_5_AF, PWM_SERVO_5_RCC_IOP);
  197. #endif
  198. #ifdef PWM_SERVO_6
  199.   set_servo_gpio(PWM_SERVO_6_GPIO, PWM_SERVO_6_PIN, PWM_SERVO_6_AF, PWM_SERVO_6_RCC_IOP);
  200. #endif
  201. #ifdef PWM_SERVO_7
  202.   set_servo_gpio(PWM_SERVO_7_GPIO, PWM_SERVO_7_PIN, PWM_SERVO_7_AF, PWM_SERVO_7_RCC_IOP);
  203. #endif
  204. #ifdef PWM_SERVO_8
  205.   set_servo_gpio(PWM_SERVO_8_GPIO, PWM_SERVO_8_PIN, PWM_SERVO_8_AF, PWM_SERVO_8_RCC_IOP);
  206. #endif
  207.  
  208. #if defined(STM32F1)
  209.   /* TIM3 GPIO for PWM1..4 */
  210.   AFIO_MAPR |= AFIO_MAPR_TIM3_REMAP_FULL_REMAP;
  211. #endif
  212.  
  213. #if PWM_USE_TIM1
  214.   set_servo_timer(TIM1, TIM1_SERVO_HZ, PWM_TIM1_CHAN_MASK);
  215. #endif
  216.  
  217. #if PWM_USE_TIM2
  218.   set_servo_timer(TIM2, TIM2_SERVO_HZ, PWM_TIM2_CHAN_MASK);
  219. #endif
  220.  
  221. #if PWM_USE_TIM3
  222.   set_servo_timer(TIM3, TIM3_SERVO_HZ, PWM_TIM3_CHAN_MASK);
  223. #endif
  224.  
  225. #if PWM_USE_TIM4
  226.   set_servo_timer(TIM4, TIM4_SERVO_HZ, PWM_TIM4_CHAN_MASK);
  227. #endif
  228.  
  229. #if PWM_USE_TIM5
  230.   set_servo_timer(TIM5, TIM5_SERVO_HZ, PWM_TIM5_CHAN_MASK);
  231. #endif
  232.  
  233. }
  234.  
  235. /** Set pulse widths from actuator values, assumed to be in us
  236.  */
  237. void actuators_pwm_commit(void) {
  238. #ifdef PWM_SERVO_0
  239.   timer_set_oc_value(PWM_SERVO_0_TIMER, PWM_SERVO_0_OC, actuators_pwm_values[PWM_SERVO_0]);
  240. #endif
  241. #ifdef PWM_SERVO_1
  242.   timer_set_oc_value(PWM_SERVO_1_TIMER, PWM_SERVO_1_OC, actuators_pwm_values[PWM_SERVO_1]);
  243. #endif
  244. #ifdef PWM_SERVO_2
  245.   timer_set_oc_value(PWM_SERVO_2_TIMER, PWM_SERVO_2_OC, actuators_pwm_values[PWM_SERVO_2]);
  246. #endif
  247. #ifdef PWM_SERVO_3
  248.   timer_set_oc_value(PWM_SERVO_3_TIMER, PWM_SERVO_3_OC, actuators_pwm_values[PWM_SERVO_3]);
  249. #endif
  250. #ifdef PWM_SERVO_4
  251.   timer_set_oc_value(PWM_SERVO_4_TIMER, PWM_SERVO_4_OC, actuators_pwm_values[PWM_SERVO_4]);
  252. #endif
  253. #ifdef PWM_SERVO_5
  254.   timer_set_oc_value(PWM_SERVO_5_TIMER, PWM_SERVO_5_OC, actuators_pwm_values[PWM_SERVO_5]);
  255. #endif
  256. #ifdef PWM_SERVO_6
  257.   timer_set_oc_value(PWM_SERVO_6_TIMER, PWM_SERVO_6_OC, actuators_pwm_values[PWM_SERVO_6]);
  258. #endif
  259. #ifdef PWM_SERVO_7
  260.   timer_set_oc_value(PWM_SERVO_7_TIMER, PWM_SERVO_7_OC, actuators_pwm_values[PWM_SERVO_7]);
  261. #endif
  262. #ifdef PWM_SERVO_8
  263.   timer_set_oc_value(PWM_SERVO_8_TIMER, PWM_SERVO_8_OC, actuators_pwm_values[PWM_SERVO_8]);
  264. #endif
  265.  
  266. }
Advertisement
RAW Paste Data Copied
Advertisement