src/main/drivers/pwm_rx.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20
  21 #include <stdlib.h>
  22
  23 #include <platform.h>
  24 #include "build_config.h"
  25 #include "debug.h"
  26
  27 #include "common/utils.h"
  28
  29 #include "system.h"
  30
  31 #include "nvic.h"
  32 #include "gpio.h"
  33 #include "timer.h"
  34
  35 #include "pwm_mapping.h"
  36
  37 #include "pwm_rx.h"
  38
  39 #define DEBUG_PPM_ISR
  40
  41 #define PPM_CAPTURE_COUNT 12
  42 #define PWM_INPUT_PORT_COUNT 8
  43
  44 #if PPM_CAPTURE_COUNT > PWM_INPUT_PORT_COUNT
  45 #define PWM_PORTS_OR_PPM_CAPTURE_COUNT PPM_CAPTURE_COUNT
  46 #else
  47 #define PWM_PORTS_OR_PPM_CAPTURE_COUNT PWM_INPUT_PORT_COUNT
  48 #endif
  49
  50 // TODO - change to timer clocks ticks
  51 #define INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX 0x03
  52
  53 pwmRxConfig_t pwmRxConfig;
  54
  55 void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity);
  56
  57 typedef enum {
  58     INPUT_MODE_PPM,
  59     INPUT_MODE_PWM,
  60 } pwmInputMode_t;
  61
  62 typedef struct {
  63     pwmInputMode_t mode;
  64     uint8_t channel; // only used for pwm, ignored by ppm
  65
  66     uint8_t state;
  67     captureCompare_t rise;
  68     captureCompare_t fall;
  69     captureCompare_t capture;
  70
  71     uint8_t missedEvents;
  72
  73     const timerHardware_t *timerHardware;
  74     timerCCHandlerRec_t edgeCb;
  75     timerOvrHandlerRec_t overflowCb;
  76 } pwmInputPort_t;
  77
  78 static pwmInputPort_t pwmInputPorts[PWM_INPUT_PORT_COUNT];
  79
  80 static uint16_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT];
  81
  82 #define PPM_TIMER_PERIOD 0x10000
  83 #define PWM_TIMER_PERIOD 0x10000
  84
  85 static uint8_t ppmFrameCount = 0;
  86 static uint8_t lastPPMFrameCount = 0;
  87 static uint8_t ppmCountShift = 0;
  88
  89 typedef struct ppmDevice_s {
  90     uint8_t  pulseIndex;
  91     //uint32_t previousTime;
  92     uint32_t currentCapture;
  93     uint32_t currentTime;
  94     uint32_t deltaTime;
  95     uint32_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT];
  96     uint32_t largeCounter;
  97     int8_t   numChannels;
  98     int8_t   numChannelsPrevFrame;
  99     uint8_t  stableFramesSeenCount;
 100
 101     bool     tracking;
 102     bool     overflowed;
 103 } ppmDevice_t;
 104
 105 ppmDevice_t ppmDev;
 106
 107
 108 #define PPM_IN_MIN_SYNC_PULSE_US    2700    // microseconds
 109 #define PPM_IN_MIN_CHANNEL_PULSE_US 750     // microseconds
 110 #define PPM_IN_MAX_CHANNEL_PULSE_US 2250    // microseconds
 111 #define PPM_STABLE_FRAMES_REQUIRED_COUNT    25
 112 #define PPM_IN_MIN_NUM_CHANNELS     4
 113 #define PPM_IN_MAX_NUM_CHANNELS     PWM_PORTS_OR_PPM_CAPTURE_COUNT
 114
 115
 116 bool isPPMDataBeingReceived(void)
 117 {
 118     return (ppmFrameCount != lastPPMFrameCount);
 119 }
 120
 121 void resetPPMDataReceivedState(void)
 122 {
 123     lastPPMFrameCount = ppmFrameCount;
 124 }
 125
 126 #define MIN_CHANNELS_BEFORE_PPM_FRAME_CONSIDERED_VALID 4
 127
 128 void pwmRxInit(void)
 129 {
 130 }
 131
 132 #ifdef DEBUG_PPM_ISR
 133 typedef enum {
 134     SOURCE_OVERFLOW = 0,
 135     SOURCE_EDGE = 1
 136 } eventSource_e;
 137
 138 typedef struct ppmISREvent_s {
 139     eventSource_e source;
 140     uint32_t capture;
 141 } ppmISREvent_t;
 142
 143 static ppmISREvent_t ppmEvents[20];
 144 static uint8_t ppmEventIndex = 0;
 145
 146 void ppmISREvent(eventSource_e source, uint32_t capture)
 147 {
 148     ppmEventIndex = (ppmEventIndex + 1) % (sizeof(ppmEvents) / sizeof(ppmEvents[0]));
 149
 150     ppmEvents[ppmEventIndex].source = source;
 151     ppmEvents[ppmEventIndex].capture = capture;
 152 }
 153 #else
 154 void ppmISREvent(eventSource_e source, uint32_t capture) {}
 155 #endif
 156
 157 static void ppmInit(void)
 158 {
 159     ppmDev.pulseIndex   = 0;
 160     ppmDev.currentCapture = 0;
 161     ppmDev.currentTime  = 0;
 162     ppmDev.deltaTime    = 0;
 163     ppmDev.largeCounter = 0;
 164     ppmDev.numChannels  = -1;
 165     ppmDev.numChannelsPrevFrame = -1;
 166     ppmDev.stableFramesSeenCount = 0;
 167     ppmDev.tracking     = false;
 168     ppmDev.overflowed   = false;
 169 }
 170
 171 static void ppmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture)
 172 {
 173     UNUSED(cbRec);
 174     ppmISREvent(SOURCE_OVERFLOW, capture);
 175
 176     ppmDev.largeCounter += capture + 1;
 177     if (capture == PPM_TIMER_PERIOD - 1) {
 178         ppmDev.overflowed = true;
 179     }
 180
 181 }
 182
 183 static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture)
 184 {
 185     UNUSED(cbRec);
 186     ppmISREvent(SOURCE_EDGE, capture);
 187
 188     int32_t i;
 189
 190     uint32_t previousTime = ppmDev.currentTime;
 191     uint32_t previousCapture = ppmDev.currentCapture;
 192
 193     /* Grab the new count */
 194     uint32_t currentTime = capture;
 195
 196     /* Convert to 32-bit timer result */
 197     currentTime += ppmDev.largeCounter;
 198
 199     if (capture < previousCapture) {
 200         if (ppmDev.overflowed) {
 201             currentTime += PPM_TIMER_PERIOD;
 202         }
 203     }
 204
 205     // Divide by 8 if Oneshot125 is active and this is a CC3D board
 206     currentTime = currentTime >> ppmCountShift;
 207
 208     /* Capture computation */
 209     if (currentTime > previousTime) {
 210         ppmDev.deltaTime    = currentTime - (previousTime + (ppmDev.overflowed ? (PPM_TIMER_PERIOD >> ppmCountShift) : 0));
 211     } else {
 212         ppmDev.deltaTime    = (PPM_TIMER_PERIOD >> ppmCountShift) + currentTime - previousTime;
 213     }
 214
 215     ppmDev.overflowed = false;
 216
 217
 218     /* Store the current measurement */
 219     ppmDev.currentTime = currentTime;
 220     ppmDev.currentCapture = capture;
 221
 222 #if 1
 223     static uint32_t deltaTimes[20];
 224     static uint8_t deltaIndex = 0;
 225
 226     deltaIndex = (deltaIndex + 1) % 20;
 227     deltaTimes[deltaIndex] = ppmDev.deltaTime;
 228     UNUSED(deltaTimes);
 229 #endif
 230
 231
 232 #if 1
 233     static uint32_t captureTimes[20];
 234     static uint8_t captureIndex = 0;
 235
 236     captureIndex = (captureIndex + 1) % 20;
 237     captureTimes[captureIndex] = capture;
 238     UNUSED(captureTimes);
 239 #endif
 240
 241     /* Sync pulse detection */
 242     if (ppmDev.deltaTime > PPM_IN_MIN_SYNC_PULSE_US) {
 243         if (ppmDev.pulseIndex == ppmDev.numChannelsPrevFrame
 244             && ppmDev.pulseIndex >= PPM_IN_MIN_NUM_CHANNELS
 245             && ppmDev.pulseIndex <= PPM_IN_MAX_NUM_CHANNELS) {
 246             /* If we see n simultaneous frames of the same
 247                number of channels we save it as our frame size */
 248             if (ppmDev.stableFramesSeenCount < PPM_STABLE_FRAMES_REQUIRED_COUNT) {
 249                 ppmDev.stableFramesSeenCount++;
 250             } else {
 251                 ppmDev.numChannels = ppmDev.pulseIndex;
 252             }
 253         } else {
 254             debug[2]++;
 255             ppmDev.stableFramesSeenCount = 0;
 256         }
 257
 258         /* Check if the last frame was well formed */
 259         if (ppmDev.pulseIndex == ppmDev.numChannels && ppmDev.tracking) {
 260             /* The last frame was well formed */
 261             for (i = 0; i < ppmDev.numChannels; i++) {
 262                 captures[i] = ppmDev.captures[i];
 263             }
 264             for (i = ppmDev.numChannels; i < PPM_IN_MAX_NUM_CHANNELS; i++) {
 265                 captures[i] = PPM_RCVR_TIMEOUT;
 266             }
 267             ppmFrameCount++;
 268         }
 269
 270         ppmDev.tracking   = true;
 271         ppmDev.numChannelsPrevFrame = ppmDev.pulseIndex;
 272         ppmDev.pulseIndex = 0;
 273
 274         /* We rely on the supervisor to set captureValue to invalid
 275            if no valid frame is found otherwise we ride over it */
 276     } else if (ppmDev.tracking) {
 277         /* Valid pulse duration 0.75 to 2.5 ms*/
 278         if (ppmDev.deltaTime > PPM_IN_MIN_CHANNEL_PULSE_US
 279             && ppmDev.deltaTime < PPM_IN_MAX_CHANNEL_PULSE_US
 280             && ppmDev.pulseIndex < PPM_IN_MAX_NUM_CHANNELS) {
 281             ppmDev.captures[ppmDev.pulseIndex] = ppmDev.deltaTime;
 282             ppmDev.pulseIndex++;
 283         } else {
 284             /* Not a valid pulse duration */
 285             ppmDev.tracking = false;
 286             for (i = 0; i < PWM_PORTS_OR_PPM_CAPTURE_COUNT; i++) {
 287                 ppmDev.captures[i] = PPM_RCVR_TIMEOUT;
 288             }
 289         }
 290     }
 291 }
 292
 293 #define MAX_MISSED_PWM_EVENTS 10
 294
 295 bool isPWMDataBeingReceived(void)
 296 {
 297     int channel;
 298     for (channel = 0; channel < PWM_PORTS_OR_PPM_CAPTURE_COUNT; channel++) {
 299         if (captures[channel] != PPM_RCVR_TIMEOUT) {
 300             return true;
 301         }
 302     }
 303     return false;
 304 }
 305
 306 static void pwmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture)
 307 {
 308     UNUSED(capture);
 309     pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, overflowCb);
 310
 311     if (++pwmInputPort->missedEvents > MAX_MISSED_PWM_EVENTS) {
 312         captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT;
 313         pwmInputPort->missedEvents = 0;
 314     }
 315 }
 316
 317 static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
 318 {
 319     pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, edgeCb);
 320     const timerHardware_t *timerHardwarePtr = pwmInputPort->timerHardware;
 321
 322     if (pwmInputPort->state == 0) {
 323         pwmInputPort->rise = capture;
 324         pwmInputPort->state = 1;
 325         pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
 326     } else {
 327         pwmInputPort->fall = capture;
 328
 329         // compute and store capture
 330         pwmInputPort->capture = pwmInputPort->fall - pwmInputPort->rise;
 331         captures[pwmInputPort->channel] = pwmInputPort->capture;
 332
 333         // switch state
 334         pwmInputPort->state = 0;
 335         pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
 336         pwmInputPort->missedEvents = 0;
 337     }
 338 }
 339
 340 static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
 341 {
 342     gpio_config_t cfg;
 343
 344     cfg.pin = pin;
 345     cfg.mode = mode;
 346     cfg.speed = Speed_2MHz;
 347     gpioInit(gpio, &cfg);
 348 }
 349
 350 void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
 351 {
 352     TIM_ICInitTypeDef TIM_ICInitStructure;
 353
 354     TIM_ICStructInit(&TIM_ICInitStructure);
 355     TIM_ICInitStructure.TIM_Channel = channel;
 356     TIM_ICInitStructure.TIM_ICPolarity = polarity;
 357     TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
 358     TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
 359
 360     if (pwmRxConfig.inputFilteringMode == INPUT_FILTERING_ENABLED) {
 361         TIM_ICInitStructure.TIM_ICFilter = INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX;
 362     } else {
 363         TIM_ICInitStructure.TIM_ICFilter = 0x00;
 364     }
 365
 366     TIM_ICInit(tim, &TIM_ICInitStructure);
 367 }
 368
 369 void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel)
 370 {
 371     pwmInputPort_t *self = &pwmInputPorts[channel];
 372
 373     self->state = 0;
 374     self->missedEvents = 0;
 375     self->channel = channel;
 376     self->mode = INPUT_MODE_PWM;
 377     self->timerHardware = timerHardwarePtr;
 378
 379     pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode);
 380     pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
 381
 382     timerConfigure(timerHardwarePtr, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_MHZ);
 383
 384     timerChCCHandlerInit(&self->edgeCb, pwmEdgeCallback);
 385     timerChOvrHandlerInit(&self->overflowCb, pwmOverflowCallback);
 386     timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb);
 387 }
 388
 389 #define UNUSED_PPM_TIMER_REFERENCE 0
 390 #define FIRST_PWM_PORT 0
 391
 392 void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer)
 393 {
 394     if (timerHardwarePtr->tim == sharedPwmTimer) {
 395         ppmCountShift = 3;  // Divide by 8 if the timer is running at 8 MHz
 396     }
 397 }
 398
 399 void ppmInConfig(const timerHardware_t *timerHardwarePtr)
 400 {
 401     ppmInit();
 402
 403     pwmInputPort_t *self = &pwmInputPorts[FIRST_PWM_PORT];
 404
 405     self->mode = INPUT_MODE_PPM;
 406     self->timerHardware = timerHardwarePtr;
 407
 408     pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode);
 409     pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
 410
 411     timerConfigure(timerHardwarePtr, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ);
 412
 413     timerChCCHandlerInit(&self->edgeCb, ppmEdgeCallback);
 414     timerChOvrHandlerInit(&self->overflowCb, ppmOverflowCallback);
 415     timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb);
 416 }
 417
 418 uint16_t ppmRead(uint8_t channel)
 419 {
 420     return captures[channel];
 421 }
 422
 423 uint16_t pwmRead(uint8_t channel)
 424 {
 425     return captures[channel];
 426 }