flight/pios/stm32f10x/pios_servo.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup   PIOS_SERVO RC Servo Functions
   6  * @brief Code to do set RC servo output
   7  * @{
   8  *
   9  * @file       pios_servo.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  11  * @brief      RC Servo routines (STM32 dependent)
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  *****************************************************************************/
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  */
  30
  31 #include "pios.h"
  32
  33 #ifdef PIOS_INCLUDE_SERVO
  34
  35 #include "pios_servo_priv.h"
  36 #include "pios_tim_priv.h"
  37
  38 /* Private Function Prototypes */
  39
  40 static const struct pios_servo_cfg *servo_cfg;
  41
  42 /**
  43  * Initialise Servos
  44  */
  45 int32_t PIOS_Servo_Init(const struct pios_servo_cfg *cfg)
  46 {
  47     uint32_t tim_id;
  48
  49     if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, NULL, 0)) {
  50         return -1;
  51     }
  52
  53     /* Store away the requested configuration */
  54     servo_cfg = cfg;
  55
  56     /* Configure the channels to be in output compare mode */
  57     for (uint8_t i = 0; i < cfg->num_channels; i++) {
  58         const struct pios_tim_channel *chan = &cfg->channels[i];
  59
  60         /* Set up for output compare function */
  61         switch (chan->timer_chan) {
  62         case TIM_Channel_1:
  63             TIM_OC1Init(chan->timer, &cfg->tim_oc_init);
  64             TIM_OC1PreloadConfig(chan->timer, TIM_OCPreload_Enable);
  65             break;
  66         case TIM_Channel_2:
  67             TIM_OC2Init(chan->timer, &cfg->tim_oc_init);
  68             TIM_OC2PreloadConfig(chan->timer, TIM_OCPreload_Enable);
  69             break;
  70         case TIM_Channel_3:
  71             TIM_OC3Init(chan->timer, &cfg->tim_oc_init);
  72             TIM_OC3PreloadConfig(chan->timer, TIM_OCPreload_Enable);
  73             break;
  74         case TIM_Channel_4:
  75             TIM_OC4Init(chan->timer, &cfg->tim_oc_init);
  76             TIM_OC4PreloadConfig(chan->timer, TIM_OCPreload_Enable);
  77             break;
  78         }
  79
  80         TIM_ARRPreloadConfig(chan->timer, ENABLE);
  81         TIM_CtrlPWMOutputs(chan->timer, ENABLE);
  82         TIM_Cmd(chan->timer, ENABLE);
  83     }
  84
  85     return 0;
  86 }
  87
  88 /**
  89  * Set the servo update rate (Max 500Hz)
  90  * \param[in] array of rates in Hz
  91  * \param[in] maximum number of banks
  92  */
  93 void PIOS_Servo_SetHz(const uint16_t *speeds, uint8_t banks)
  94 {
  95     if (!servo_cfg) {
  96         return;
  97     }
  98
  99     TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure = servo_cfg->tim_base_init;
 100     TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
 101     TIM_TimeBaseStructure.TIM_CounterMode   = TIM_CounterMode_Up;
 102     TIM_TimeBaseStructure.TIM_Prescaler     = (PIOS_MASTER_CLOCK / 1000000) - 1;
 103
 104     uint8_t set = 0;
 105
 106     for (uint8_t i = 0; (i < servo_cfg->num_channels) && (set < banks); i++) {
 107         bool new = true;
 108         const struct pios_tim_channel *chan = &servo_cfg->channels[i];
 109
 110         /* See if any previous channels use that same timer */
 111         for (uint8_t j = 0; (j < i) && new; j++) {
 112             new &= chan->timer != servo_cfg->channels[j].timer;
 113         }
 114
 115         if (new) {
 116             TIM_TimeBaseStructure.TIM_Period = ((1000000 / speeds[set]) - 1);
 117             TIM_TimeBaseInit(chan->timer, &TIM_TimeBaseStructure);
 118             set++;
 119         }
 120     }
 121 }
 122
 123 /**
 124  * Set servo position
 125  * \param[in] Servo Servo number (0-7)
 126  * \param[in] Position Servo position in microseconds
 127  */
 128 void PIOS_Servo_Set(uint8_t servo, uint16_t position)
 129 {
 130     /* Make sure servo exists */
 131     if (!servo_cfg || servo >= servo_cfg->num_channels) {
 132         return;
 133     }
 134
 135     /* Update the position */
 136     const struct pios_tim_channel *chan = &servo_cfg->channels[servo];
 137     switch (chan->timer_chan) {
 138     case TIM_Channel_1:
 139         TIM_SetCompare1(chan->timer, position);
 140         break;
 141     case TIM_Channel_2:
 142         TIM_SetCompare2(chan->timer, position);
 143         break;
 144     case TIM_Channel_3:
 145         TIM_SetCompare3(chan->timer, position);
 146         break;
 147     case TIM_Channel_4:
 148         TIM_SetCompare4(chan->timer, position);
 149         break;
 150     }
 151 }
 152
 153 #endif /* PIOS_INCLUDE_SERVO */