flight/pios/stm32f4xx/pios_ppm.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  * @addtogroup   PIOS_PPM PPM Input Functions
   6  * @brief Code to measure PPM input and seperate into channels
   7  * @{
   8  *
   9  * @file       pios_ppm.c
  10  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  11  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  12  * @brief      PPM Input functions (STM32 dependent)
  13  * @see        The GNU Public License (GPL) Version 3
  14  *
  15  *****************************************************************************/
  16 /*
  17  * This program is free software; you can redistribute it and/or modify
  18  * it under the terms of the GNU General Public License as published by
  19  * the Free Software Foundation; either version 3 of the License, or
  20  * (at your option) any later version.
  21  *
  22  * This program is distributed in the hope that it will be useful, but
  23  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  24  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  25  * for more details.
  26  *
  27  * You should have received a copy of the GNU General Public License along
  28  * with this program; if not, write to the Free Software Foundation, Inc.,
  29  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  30  */
  31
  32 #include "pios.h"
  33
  34 #ifdef PIOS_INCLUDE_PPM
  35
  36 #include "pios_ppm_priv.h"
  37
  38 /* Provide a RCVR driver */
  39 static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel);
  40 static xSemaphoreHandle PIOS_PPM_Get_Semaphore(uint32_t rcvr_id, uint8_t channel);
  41
  42 const struct pios_rcvr_driver pios_ppm_rcvr_driver = {
  43     .read          = PIOS_PPM_Get,
  44 #if defined(PIOS_INCLUDE_FREERTOS)
  45     .get_semaphore = PIOS_PPM_Get_Semaphore
  46 #endif
  47 };
  48
  49 #define PIOS_PPM_IN_MIN_NUM_CHANNELS     4
  50 #define PIOS_PPM_IN_MAX_NUM_CHANNELS     PIOS_PPM_NUM_INPUTS
  51 #define PIOS_PPM_STABLE_CHANNEL_COUNT    25      // frames
  52 #define PIOS_PPM_IN_MIN_SYNC_PULSE_US    2700    // microseconds
  53 #define PIOS_PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds
  54 #define PIOS_PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds
  55
  56 static void PIOS_PPM_Supervisor(uint32_t ppm_id);
  57
  58 enum pios_ppm_dev_magic {
  59     PIOS_PPM_DEV_MAGIC = 0xee014d8b,
  60 };
  61
  62 struct pios_ppm_dev {
  63     enum pios_ppm_dev_magic   magic;
  64     const struct pios_ppm_cfg *cfg;
  65
  66     uint8_t  PulseIndex;
  67     uint32_t PreviousTime;
  68     uint32_t CurrentTime;
  69     uint32_t DeltaTime;
  70     uint32_t CaptureValue[PIOS_PPM_IN_MAX_NUM_CHANNELS];
  71     uint32_t CaptureValueNewFrame[PIOS_PPM_IN_MAX_NUM_CHANNELS];
  72     uint32_t LargeCounter;
  73     int8_t   NumChannels;
  74     int8_t   NumChannelsPrevFrame;
  75     uint8_t  NumChannelCounter;
  76
  77     uint8_t  supv_timer;
  78     bool     Tracking;
  79     bool     Fresh;
  80
  81 #ifdef PIOS_INCLUDE_FREERTOS
  82     xSemaphoreHandle new_sample_semaphores[PIOS_PPM_IN_MIN_NUM_CHANNELS];
  83 #endif /* PIOS_INCLUDE_FREERTOS */
  84 };
  85
  86 static bool PIOS_PPM_validate(struct pios_ppm_dev *ppm_dev)
  87 {
  88     return ppm_dev->magic == PIOS_PPM_DEV_MAGIC;
  89 }
  90
  91 #if defined(PIOS_INCLUDE_FREERTOS)
  92 static struct pios_ppm_dev *PIOS_PPM_alloc(void)
  93 {
  94     struct pios_ppm_dev *ppm_dev;
  95
  96     ppm_dev = (struct pios_ppm_dev *)pios_malloc(sizeof(*ppm_dev));
  97     if (!ppm_dev) {
  98         return NULL;
  99     }
 100
 101     // Initialize the semaphores to 0.
 102     for (uint8_t i = 0; i < PIOS_PPM_IN_MIN_NUM_CHANNELS; ++i) {
 103         ppm_dev->new_sample_semaphores[i] = 0;
 104     }
 105
 106     ppm_dev->magic = PIOS_PPM_DEV_MAGIC;
 107     return ppm_dev;
 108 }
 109 #else
 110 static struct pios_ppm_dev pios_ppm_devs[PIOS_PPM_MAX_DEVS];
 111 static uint8_t pios_ppm_num_devs;
 112 static struct pios_ppm_dev *PIOS_PPM_alloc(void)
 113 {
 114     struct pios_ppm_dev *ppm_dev;
 115
 116     if (pios_ppm_num_devs >= PIOS_PPM_MAX_DEVS) {
 117         return NULL;
 118     }
 119
 120     ppm_dev = &pios_ppm_devs[pios_ppm_num_devs++];
 121     ppm_dev->magic = PIOS_PPM_DEV_MAGIC;
 122
 123     return ppm_dev;
 124 }
 125 #endif /* if defined(PIOS_INCLUDE_FREERTOS) */
 126
 127 static void PIOS_PPM_tim_overflow_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
 128 static void PIOS_PPM_tim_edge_cb(uint32_t id, uint32_t context, uint8_t channel, uint16_t count);
 129 static const struct pios_tim_callbacks tim_callbacks = {
 130     .overflow = PIOS_PPM_tim_overflow_cb,
 131     .edge     = PIOS_PPM_tim_edge_cb,
 132 };
 133
 134 extern int32_t PIOS_PPM_Init(uint32_t *ppm_id, const struct pios_ppm_cfg *cfg)
 135 {
 136     PIOS_DEBUG_Assert(ppm_id);
 137     PIOS_DEBUG_Assert(cfg);
 138
 139     struct pios_ppm_dev *ppm_dev;
 140
 141     ppm_dev = (struct pios_ppm_dev *)PIOS_PPM_alloc();
 142     if (!ppm_dev) {
 143         goto out_fail;
 144     }
 145
 146     /* Bind the configuration to the device instance */
 147     ppm_dev->cfg = cfg;
 148
 149     /* Set up the state variables */
 150     ppm_dev->PulseIndex   = 0;
 151     ppm_dev->PreviousTime = 0;
 152     ppm_dev->CurrentTime  = 0;
 153     ppm_dev->DeltaTime    = 0;
 154     ppm_dev->LargeCounter = 0;
 155     ppm_dev->NumChannels  = -1;
 156     ppm_dev->NumChannelsPrevFrame = -1;
 157     ppm_dev->NumChannelCounter = 0;
 158     ppm_dev->Tracking     = false;
 159     ppm_dev->Fresh = false;
 160
 161     for (uint8_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
 162         /* Flush counter variables */
 163         ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
 164         ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT;
 165     }
 166
 167     uint32_t tim_id;
 168     if (PIOS_TIM_InitChannels(&tim_id, cfg->channels, cfg->num_channels, &tim_callbacks, (uint32_t)ppm_dev)) {
 169         return -1;
 170     }
 171
 172     TIM_ICInitTypeDef TIM_ICInitStructure = cfg->tim_ic_init;
 173
 174     /* Configure the channels to be in capture/compare mode */
 175     for (uint8_t i = 0; i < cfg->num_channels; i++) {
 176         const struct pios_tim_channel *chan = &cfg->channels[i];
 177
 178         /* Configure timer for input capture */
 179         TIM_ICInitStructure.TIM_Channel = chan->timer_chan;
 180         TIM_ICInit(chan->timer, &TIM_ICInitStructure);
 181
 182         /* Enable the Capture Compare Interrupt Request */
 183         switch (chan->timer_chan) {
 184         case TIM_Channel_1:
 185             TIM_ITConfig(chan->timer, TIM_IT_CC1 | TIM_IT_Update, ENABLE);
 186             break;
 187         case TIM_Channel_2:
 188             TIM_ITConfig(chan->timer, TIM_IT_CC2 | TIM_IT_Update, ENABLE);
 189             break;
 190         case TIM_Channel_3:
 191             TIM_ITConfig(chan->timer, TIM_IT_CC3 | TIM_IT_Update, ENABLE);
 192             break;
 193         case TIM_Channel_4:
 194             TIM_ITConfig(chan->timer, TIM_IT_CC4 | TIM_IT_Update, ENABLE);
 195             break;
 196         }
 197     }
 198
 199     if (!PIOS_RTC_RegisterTickCallback(PIOS_PPM_Supervisor, (uint32_t)ppm_dev)) {
 200         PIOS_DEBUG_Assert(0);
 201     }
 202
 203     *ppm_id = (uint32_t)ppm_dev;
 204
 205     return 0;
 206
 207 out_fail:
 208     return -1;
 209 }
 210
 211 #if defined(PIOS_INCLUDE_FREERTOS)
 212 static xSemaphoreHandle PIOS_PPM_Get_Semaphore(uint32_t rcvr_id, uint8_t channel)
 213 {
 214     struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)rcvr_id;
 215
 216     if (!PIOS_PPM_validate(ppm_dev)) {
 217         /* Invalid device specified */
 218         return 0;
 219     }
 220
 221     if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) {
 222         /* Channel out of range */
 223         return 0;
 224     }
 225
 226     if (ppm_dev->new_sample_semaphores[channel] == 0) {
 227         vSemaphoreCreateBinary(ppm_dev->new_sample_semaphores[channel]);
 228     }
 229     return ppm_dev->new_sample_semaphores[channel];
 230 }
 231 #endif /* if defined(PIOS_INCLUDE_FREERTOS) */
 232
 233 /**
 234  * Get the value of an input channel
 235  * \param[in] channel Number of the channel desired (zero based)
 236  * \output PIOS_RCVR_INVALID channel not available
 237  * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver
 238  * \output >=0 channel value
 239  */
 240 static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel)
 241 {
 242     struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)rcvr_id;
 243
 244     if (!PIOS_PPM_validate(ppm_dev)) {
 245         /* Invalid device specified */
 246         return PIOS_RCVR_INVALID;
 247     }
 248
 249     if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) {
 250         /* Channel out of range */
 251         return PIOS_RCVR_INVALID;
 252     }
 253     return ppm_dev->CaptureValue[channel];
 254 }
 255
 256 static void PIOS_PPM_tim_overflow_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context,
 257                                      __attribute__((unused)) uint8_t channel, uint16_t count)
 258 {
 259     struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)context;
 260
 261     if (!PIOS_PPM_validate(ppm_dev)) {
 262         /* Invalid device specified */
 263         return;
 264     }
 265
 266     ppm_dev->LargeCounter += count;
 267 }
 268
 269
 270 static void PIOS_PPM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32_t context, uint8_t chan_idx, uint16_t count)
 271 {
 272     /* Recover our device context */
 273     struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)context;
 274
 275     if (!PIOS_PPM_validate(ppm_dev)) {
 276         /* Invalid device specified */
 277         return;
 278     }
 279
 280     if (chan_idx >= ppm_dev->cfg->num_channels) {
 281         /* Channel out of range */
 282         return;
 283     }
 284
 285     /* Shift the last measurement out */
 286     ppm_dev->PreviousTime = ppm_dev->CurrentTime;
 287
 288     /* Grab the new count */
 289     ppm_dev->CurrentTime  = count;
 290
 291     /* Convert to 32-bit timer result */
 292     ppm_dev->CurrentTime += ppm_dev->LargeCounter;
 293
 294     /* Capture computation */
 295     ppm_dev->DeltaTime    = ppm_dev->CurrentTime - ppm_dev->PreviousTime;
 296
 297     ppm_dev->PreviousTime = ppm_dev->CurrentTime;
 298
 299     /* Sync pulse detection */
 300     if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_SYNC_PULSE_US) {
 301         if (ppm_dev->PulseIndex == ppm_dev->NumChannelsPrevFrame
 302             && ppm_dev->PulseIndex >= PIOS_PPM_IN_MIN_NUM_CHANNELS
 303             && ppm_dev->PulseIndex <= PIOS_PPM_IN_MAX_NUM_CHANNELS) {
 304             /* If we see n simultaneous frames of the same
 305                number of channels we save it as our frame size */
 306             if (ppm_dev->NumChannelCounter < PIOS_PPM_STABLE_CHANNEL_COUNT) {
 307                 ppm_dev->NumChannelCounter++;
 308             } else {
 309                 ppm_dev->NumChannels = ppm_dev->PulseIndex;
 310             }
 311         } else {
 312             ppm_dev->NumChannelCounter = 0;
 313         }
 314
 315         /* Check if the last frame was well formed */
 316         if (ppm_dev->PulseIndex == ppm_dev->NumChannels && ppm_dev->Tracking) {
 317             /* The last frame was well formed */
 318             for (int32_t i = 0; i < ppm_dev->NumChannels; i++) {
 319                 ppm_dev->CaptureValue[i] = ppm_dev->CaptureValueNewFrame[i];
 320             }
 321             for (uint32_t i = ppm_dev->NumChannels;
 322                  i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
 323                 ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
 324             }
 325 #if defined(PIOS_INCLUDE_FREERTOS)
 326             /* Signal that a new sample is ready on this channel. */
 327             if (ppm_dev->new_sample_semaphores[chan_idx] != 0) {
 328                 signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE;
 329                 if (xSemaphoreGiveFromISR(ppm_dev->new_sample_semaphores[chan_idx], &pxHigherPriorityTaskWoken) == pdTRUE) {
 330                     portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for his? */
 331                 }
 332             }
 333 #endif /* USE_FREERTOS */
 334         } else {
 335             for (uint32_t i = 0;
 336                  i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
 337                 ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
 338             }
 339         }
 340
 341         ppm_dev->Fresh      = true;
 342         ppm_dev->Tracking   = true;
 343         ppm_dev->NumChannelsPrevFrame = ppm_dev->PulseIndex;
 344         ppm_dev->PulseIndex = 0;
 345
 346         /* We rely on the supervisor to set CaptureValue to invalid
 347            if no valid frame is found otherwise we ride over it */
 348     } else if (ppm_dev->Tracking) {
 349         /* Valid pulse duration 0.75 to 2.5 ms*/
 350         if (ppm_dev->DeltaTime > PIOS_PPM_IN_MIN_CHANNEL_PULSE_US
 351             && ppm_dev->DeltaTime < PIOS_PPM_IN_MAX_CHANNEL_PULSE_US
 352             && ppm_dev->PulseIndex < PIOS_PPM_IN_MAX_NUM_CHANNELS) {
 353             ppm_dev->CaptureValueNewFrame[ppm_dev->PulseIndex] = ppm_dev->DeltaTime;
 354             ppm_dev->PulseIndex++;
 355         } else {
 356             /* Not a valid pulse duration */
 357             ppm_dev->Tracking = false;
 358             for (uint32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
 359                 ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT;
 360             }
 361         }
 362     }
 363 }
 364
 365 static void PIOS_PPM_Supervisor(uint32_t ppm_id)
 366 {
 367     /* Recover our device context */
 368     struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)ppm_id;
 369
 370     if (!PIOS_PPM_validate(ppm_dev)) {
 371         /* Invalid device specified */
 372         return;
 373     }
 374
 375     /*
 376      * RTC runs at 625Hz so divide down the base rate so
 377      * that this loop runs at 25Hz.
 378      */
 379     if (++(ppm_dev->supv_timer) < 25) {
 380         return;
 381     }
 382     ppm_dev->supv_timer = 0;
 383
 384     if (!ppm_dev->Fresh) {
 385         ppm_dev->Tracking = false;
 386
 387         for (int32_t i = 0; i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) {
 388             ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT;
 389             ppm_dev->CaptureValueNewFrame[i] = PIOS_RCVR_TIMEOUT;
 390         }
 391     }
 392
 393     ppm_dev->Fresh = false;
 394 }
 395
 396 #endif /* PIOS_INCLUDE_PPM */
 397
 398 /**
 399  * @}
 400  * @}
 401  */