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