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