flight/pios/stm32f4xx/pios_pwm.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup   PIOS_PWM PWM Input Functions
   6  * @brief Code to measure with PWM input
   7  * @{
   8  *
   9  * @file       pios_pwm.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  11  * @brief      PWM Input functions (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_PWM
  34
  35 #include "pios_pwm_priv.h"
  36
  37 /* Provide a RCVR driver */
  38 static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel);
  39
  40 const struct pios_rcvr_driver pios_pwm_rcvr_driver = {
  41     .read = PIOS_PWM_Get,
  42 };
  43
  44 /* Local Variables */
  45 /* 100 ms timeout without updates on channels */
  46 static const uint32_t PWM_SUPERVISOR_TIMEOUT = 100000;
  47
  48 enum pios_pwm_dev_magic {
  49     PIOS_PWM_DEV_MAGIC = 0xab30293c,
  50 };
  51
  52 struct pios_pwm_dev {
  53     enum pios_pwm_dev_magic   magic;
  54     const struct pios_pwm_cfg *cfg;
  55
  56     uint8_t  CaptureState[PIOS_PWM_NUM_INPUTS];
  57     uint16_t RiseValue[PIOS_PWM_NUM_INPUTS];
  58     uint16_t FallValue[PIOS_PWM_NUM_INPUTS];
  59     uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS];
  60     uint32_t CapCounter[PIOS_PWM_NUM_INPUTS];
  61     uint32_t us_since_update[PIOS_PWM_NUM_INPUTS];
  62 };
  63
  64 static bool PIOS_PWM_validate(struct pios_pwm_dev *pwm_dev)
  65 {
  66     return pwm_dev->magic == PIOS_PWM_DEV_MAGIC;
  67 }
  68
  69 #if defined(PIOS_INCLUDE_FREERTOS)
  70 static struct pios_pwm_dev *PIOS_PWM_alloc(void)
  71 {
  72     struct pios_pwm_dev *pwm_dev;
  73
  74     pwm_dev = (struct pios_pwm_dev *)pios_malloc(sizeof(*pwm_dev));
  75     if (!pwm_dev) {
  76         return NULL;
  77     }
  78
  79     pwm_dev->magic = PIOS_PWM_DEV_MAGIC;
  80     return pwm_dev;
  81 }
  82 #else
  83 static struct pios_pwm_dev pios_pwm_devs[PIOS_PWM_MAX_DEVS];
  84 static uint8_t pios_pwm_num_devs;
  85 static struct pios_pwm_dev *PIOS_PWM_alloc(void)
  86 {
  87     struct pios_pwm_dev *pwm_dev;
  88
  89     if (pios_pwm_num_devs >= PIOS_PWM_MAX_DEVS) {
  90         return NULL;
  91     }
  92
  93     pwm_dev = &pios_pwm_devs[pios_pwm_num_devs++];
  94     pwm_dev->magic = PIOS_PWM_DEV_MAGIC;
  95
  96     return pwm_dev;
  97 }
  98 #endif /* if defined(PIOS_INCLUDE_FREERTOS) */
  99
 100 static void PIOS_PWM_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
 101 static void PIOS_PWM_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
 102 static const struct pios_tim_callbacks tim_callbacks = {
 103     .overflow = PIOS_PWM_tim_overflow_cb,
 104     .edge     = PIOS_PWM_tim_edge_cb,
 105 };
 106
 107 /**
 108  * Initialises all the pins
 109  */
 110 int32_t PIOS_PWM_Init(uint32_t *pwm_id, const struct pios_pwm_cfg *cfg)
 111 {
 112     PIOS_DEBUG_Assert(pwm_id);
 113     PIOS_DEBUG_Assert(cfg);
 114
 115     struct pios_pwm_dev *pwm_dev;
 116
 117     pwm_dev = (struct pios_pwm_dev *)PIOS_PWM_alloc();
 118     if (!pwm_dev) {
 119         goto out_fail;
 120     }
 121
 122     /* Bind the configuration to the device instance */
 123     pwm_dev->cfg = cfg;
 124
 125     for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) {
 126         /* Flush counter variables */
 127         pwm_dev->CaptureState[i] = 0;
 128         pwm_dev->RiseValue[i]    = 0;
 129         pwm_dev->FallValue[i]    = 0;
 130         pwm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
 131     }
 132
 133     uint32_t tim_id;
 134     if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)pwm_dev)) {
 135         return -1;
 136     }
 137
 138     /* Configure the channels to be in capture/compare mode */
 139     for (uint8_t i = 0; i < cfg->num_channels; i++) {
 140         const struct pios_tim_channel *chan   = &cfg->channels[i];
 141
 142         /* Configure timer for input capture */
 143         TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
 144         TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
 145         TIM_ICInit(chan->timer, &TIM_ICInitStructure);
 146
 147         /* Enable the Capture Compare Interrupt Request */
 148         switch (chan->timer_chan) {
 149         case TIM_Channel_1:
 150             TIM_ITConfig(chan->timer, TIM_IT_CC1, ENABLE);
 151             break;
 152         case TIM_Channel_2:
 153             TIM_ITConfig(chan->timer, TIM_IT_CC2, ENABLE);
 154             break;
 155         case TIM_Channel_3:
 156             TIM_ITConfig(chan->timer, TIM_IT_CC3, ENABLE);
 157             break;
 158         case TIM_Channel_4:
 159             TIM_ITConfig(chan->timer, TIM_IT_CC4, ENABLE);
 160             break;
 161         }
 162
 163         // Need the update event for that timer to detect timeouts
 164         TIM_ITConfig(chan->timer, TIM_IT_Update, ENABLE);
 165     }
 166
 167     *pwm_id = (uint32_t)pwm_dev;
 168
 169     return 0;
 170
 171 out_fail:
 172     return -1;
 173 }
 174
 175 /**
 176  * Get the value of an input channel
 177  * \param[in] channel Number of the channel desired (zero based)
 178  * \output PIOS_RCVR_INVALID channel not available
 179  * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver
 180  * \output >=0 channel value
 181  */
 182 static int32_t PIOS_PWM_Get(uint32_t rcvr_id, uint8_t channel)
 183 {
 184     struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)rcvr_id;
 185
 186     if (!PIOS_PWM_validate(pwm_dev)) {
 187         /* Invalid device specified */
 188         return PIOS_RCVR_INVALID;
 189     }
 190
 191     if (channel >= PIOS_PWM_NUM_INPUTS) {
 192         /* Channel out of range */
 193         return PIOS_RCVR_INVALID;
 194     }
 195     return pwm_dev->CaptureValue[channel];
 196 }
 197
 198 static void PIOS_PWM_tim_overflow_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t channel, uint16_t count)
 199 {
 200     struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)context;
 201
 202     if (!PIOS_PWM_validate(pwm_dev)) {
 203         /* Invalid device specified */
 204         return;
 205     }
 206
 207     if (channel >= pwm_dev->cfg->num_channels) {
 208         /* Channel out of range */
 209         return;
 210     }
 211
 212     pwm_dev->us_since_update[channel] += count;
 213     if (pwm_dev->us_since_update[channel] >= PWM_SUPERVISOR_TIMEOUT) {
 214         pwm_dev->CaptureState[channel]    = 0;
 215         pwm_dev->RiseValue[channel]       = 0;
 216         pwm_dev->FallValue[channel]       = 0;
 217         pwm_dev->CaptureValue[channel]    = PIOS_RCVR_TIMEOUT;
 218         pwm_dev->us_since_update[channel] = 0;
 219     }
 220 }
 221
 222 static void PIOS_PWM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
 223 {
 224     /* Recover our device context */
 225     struct pios_pwm_dev *pwm_dev = (struct pios_pwm_dev *)context;
 226
 227     if (!PIOS_PWM_validate(pwm_dev)) {
 228         /* Invalid device specified */
 229         return;
 230     }
 231
 232     if (chan_idx >= pwm_dev->cfg->num_channels) {
 233         /* Channel out of range */
 234         return;
 235     }
 236
 237     const struct pios_tim_channel *chan = &pwm_dev->cfg->channels[chan_idx];
 238
 239     if (pwm_dev->CaptureState[chan_idx] == 0) {
 240         pwm_dev->RiseValue[chan_idx] = count;
 241         pwm_dev->us_since_update[chan_idx] = 0;
 242     } else {
 243         pwm_dev->FallValue[chan_idx] = count;
 244     }
 245
 246     // flip state machine and capture value here
 247     /* Simple rise or fall state machine */
 248     TIM_ICInitTypeDef TIM_ICInitStructure = pwm_dev->cfg->tim_ic_init;
 249     if (pwm_dev->CaptureState[chan_idx] == 0) {
 250         /* Switch states */
 251         pwm_dev->CaptureState[chan_idx]    = 1;
 252
 253         /* Switch polarity of input capture */
 254         TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling;
 255         TIM_ICInitStructure.TIM_Channel    = chan->timer_chan;
 256         TIM_ICInit(chan->timer, &TIM_ICInitStructure);
 257     } else {
 258         /* Capture computation */
 259         if (pwm_dev->FallValue[chan_idx] > pwm_dev->RiseValue[chan_idx]) {
 260             pwm_dev->CaptureValue[chan_idx] = (pwm_dev->FallValue[chan_idx] - pwm_dev->RiseValue[chan_idx]);
 261         } else {
 262             pwm_dev->CaptureValue[chan_idx] = ((chan->timer->ARR - pwm_dev->RiseValue[chan_idx]) + pwm_dev->FallValue[chan_idx]);
 263         }
 264
 265         /* Switch states */
 266         pwm_dev->CaptureState[chan_idx] = 0;
 267
 268         /* Increase supervisor counter */
 269         pwm_dev->CapCounter[chan_idx]++;
 270
 271         /* Switch polarity of input capture */
 272         TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
 273         TIM_ICInitStructure.TIM_Channel    = chan->timer_chan;
 274         TIM_ICInit(chan->timer, &TIM_ICInitStructure);
 275     }
 276 }
 277
 278 #endif /* PIOS_INCLUDE_PWM */
 279
 280 /**
 281  * @}
 282  * @}
 283  */