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 > MAX_PWM_INPUT_PORTS
  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 static inputFilteringMode_e inputFilteringMode;
  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(inputFilteringMode_e initialInputFilteringMode)
 129 {
 130     inputFilteringMode = initialInputFilteringMode;
 131 }
 132
 133 #ifdef DEBUG_PPM_ISR
 134 typedef enum {
 135     SOURCE_OVERFLOW = 0,
 136     SOURCE_EDGE = 1
 137 } eventSource_e;
 138
 139 typedef struct ppmISREvent_s {
 140     eventSource_e source;
 141     uint32_t capture;
 142 } ppmISREvent_t;
 143
 144 static ppmISREvent_t ppmEvents[20];
 145 static uint8_t ppmEventIndex = 0;
 146
 147 void ppmISREvent(eventSource_e source, uint32_t capture)
 148 {
 149     ppmEventIndex = (ppmEventIndex + 1) % (sizeof(ppmEvents) / sizeof(ppmEvents[0]));
 150
 151     ppmEvents[ppmEventIndex].source = source;
 152     ppmEvents[ppmEventIndex].capture = capture;
 153 }
 154 #else
 155 void ppmISREvent(eventSource_e source, uint32_t capture) {}
 156 #endif
 157
 158 static void ppmInit(void)
 159 {
 160     ppmDev.pulseIndex   = 0;
 161     ppmDev.currentCapture = 0;
 162     ppmDev.currentTime  = 0;
 163     ppmDev.deltaTime    = 0;
 164     ppmDev.largeCounter = 0;
 165     ppmDev.numChannels  = -1;
 166     ppmDev.numChannelsPrevFrame = -1;
 167     ppmDev.stableFramesSeenCount = 0;
 168     ppmDev.tracking     = false;
 169     ppmDev.overflowed   = false;
 170 }
 171
 172 static void ppmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture)
 173 {
 174     UNUSED(cbRec);
 175     ppmISREvent(SOURCE_OVERFLOW, capture);
 176
 177     ppmDev.largeCounter += capture + 1;
 178     if (capture == PPM_TIMER_PERIOD - 1) {
 179         ppmDev.overflowed = true;
 180     }
 181
 182 }
 183
 184 static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture)
 185 {
 186     UNUSED(cbRec);
 187     ppmISREvent(SOURCE_EDGE, capture);
 188
 189     int32_t i;
 190
 191     uint32_t previousTime = ppmDev.currentTime;
 192     uint32_t previousCapture = ppmDev.currentCapture;
 193
 194     /* Grab the new count */
 195     uint32_t currentTime = capture;
 196
 197     /* Convert to 32-bit timer result */
 198     currentTime += ppmDev.largeCounter;
 199
 200     if (capture < previousCapture) {
 201         if (ppmDev.overflowed) {
 202             currentTime += PPM_TIMER_PERIOD;
 203         }
 204     }
 205
 206     // Divide by 8 if Oneshot125 is active and this is a CC3D board
 207     currentTime = currentTime >> ppmCountShift;
 208
 209     /* Capture computation */
 210     if (currentTime > previousTime) {
 211         ppmDev.deltaTime    = currentTime - (previousTime + (ppmDev.overflowed ? (PPM_TIMER_PERIOD >> ppmCountShift) : 0));
 212     } else {
 213         ppmDev.deltaTime    = (PPM_TIMER_PERIOD >> ppmCountShift) + currentTime - previousTime;
 214     }
 215
 216     ppmDev.overflowed = false;
 217
 218
 219     /* Store the current measurement */
 220     ppmDev.currentTime = currentTime;
 221     ppmDev.currentCapture = capture;
 222
 223 #if 1
 224     static uint32_t deltaTimes[20];
 225     static uint8_t deltaIndex = 0;
 226
 227     deltaIndex = (deltaIndex + 1) % 20;
 228     deltaTimes[deltaIndex] = ppmDev.deltaTime;
 229     UNUSED(deltaTimes);
 230 #endif
 231
 232
 233 #if 1
 234     static uint32_t captureTimes[20];
 235     static uint8_t captureIndex = 0;
 236
 237     captureIndex = (captureIndex + 1) % 20;
 238     captureTimes[captureIndex] = capture;
 239     UNUSED(captureTimes);
 240 #endif
 241
 242     /* Sync pulse detection */
 243     if (ppmDev.deltaTime > PPM_IN_MIN_SYNC_PULSE_US) {
 244         if (ppmDev.pulseIndex == ppmDev.numChannelsPrevFrame
 245             && ppmDev.pulseIndex >= PPM_IN_MIN_NUM_CHANNELS
 246             && ppmDev.pulseIndex <= PPM_IN_MAX_NUM_CHANNELS) {
 247             /* If we see n simultaneous frames of the same
 248                number of channels we save it as our frame size */
 249             if (ppmDev.stableFramesSeenCount < PPM_STABLE_FRAMES_REQUIRED_COUNT) {
 250                 ppmDev.stableFramesSeenCount++;
 251             } else {
 252                 ppmDev.numChannels = ppmDev.pulseIndex;
 253             }
 254         } else {
 255             debug[2]++;
 256             ppmDev.stableFramesSeenCount = 0;
 257         }
 258
 259         /* Check if the last frame was well formed */
 260         if (ppmDev.pulseIndex == ppmDev.numChannels && ppmDev.tracking) {
 261             /* The last frame was well formed */
 262             for (i = 0; i < ppmDev.numChannels; i++) {
 263                 captures[i] = ppmDev.captures[i];
 264             }
 265             for (i = ppmDev.numChannels; i < PPM_IN_MAX_NUM_CHANNELS; i++) {
 266                 captures[i] = PPM_RCVR_TIMEOUT;
 267             }
 268             ppmFrameCount++;
 269         }
 270
 271         ppmDev.tracking   = true;
 272         ppmDev.numChannelsPrevFrame = ppmDev.pulseIndex;
 273         ppmDev.pulseIndex = 0;
 274
 275         /* We rely on the supervisor to set captureValue to invalid
 276            if no valid frame is found otherwise we ride over it */
 277     } else if (ppmDev.tracking) {
 278         /* Valid pulse duration 0.75 to 2.5 ms*/
 279         if (ppmDev.deltaTime > PPM_IN_MIN_CHANNEL_PULSE_US
 280             && ppmDev.deltaTime < PPM_IN_MAX_CHANNEL_PULSE_US
 281             && ppmDev.pulseIndex < PPM_IN_MAX_NUM_CHANNELS) {
 282             ppmDev.captures[ppmDev.pulseIndex] = ppmDev.deltaTime;
 283             ppmDev.pulseIndex++;
 284         } else {
 285             /* Not a valid pulse duration */
 286             ppmDev.tracking = false;
 287             for (i = 0; i < PWM_PORTS_OR_PPM_CAPTURE_COUNT; i++) {
 288                 ppmDev.captures[i] = PPM_RCVR_TIMEOUT;
 289             }
 290         }
 291     }
 292 }
 293
 294 #define MAX_MISSED_PWM_EVENTS 10
 295
 296 bool isPWMDataBeingReceived(void)
 297 {
 298     int channel;
 299     for (channel = 0; channel < PWM_PORTS_OR_PPM_CAPTURE_COUNT; channel++) {
 300         if (captures[channel] != PPM_RCVR_TIMEOUT) {
 301             return true;
 302         }
 303     }
 304     return false;
 305 }
 306
 307 static void pwmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture)
 308 {
 309     UNUSED(capture);
 310     pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, overflowCb);
 311
 312     if (++pwmInputPort->missedEvents > MAX_MISSED_PWM_EVENTS) {
 313         captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT;
 314         pwmInputPort->missedEvents = 0;
 315     }
 316 }
 317
 318 static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
 319 {
 320     pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, edgeCb);
 321     const timerHardware_t *timerHardwarePtr = pwmInputPort->timerHardware;
 322
 323     if (pwmInputPort->state == 0) {
 324         pwmInputPort->rise = capture;
 325         pwmInputPort->state = 1;
 326         pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
 327     } else {
 328         pwmInputPort->fall = capture;
 329
 330         // compute and store capture
 331         pwmInputPort->capture = pwmInputPort->fall - pwmInputPort->rise;
 332         captures[pwmInputPort->channel] = pwmInputPort->capture;
 333
 334         // switch state
 335         pwmInputPort->state = 0;
 336         pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
 337         pwmInputPort->missedEvents = 0;
 338     }
 339 }
 340
 341 static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
 342 {
 343     gpio_config_t cfg;
 344
 345     cfg.pin = pin;
 346     cfg.mode = mode;
 347     cfg.speed = Speed_2MHz;
 348     gpioInit(gpio, &cfg);
 349 }
 350
 351 void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
 352 {
 353     TIM_ICInitTypeDef TIM_ICInitStructure;
 354
 355     TIM_ICStructInit(&TIM_ICInitStructure);
 356     TIM_ICInitStructure.TIM_Channel = channel;
 357     TIM_ICInitStructure.TIM_ICPolarity = polarity;
 358     TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
 359     TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
 360
 361     if (inputFilteringMode == INPUT_FILTERING_ENABLED) {
 362         TIM_ICInitStructure.TIM_ICFilter = INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX;
 363     } else {
 364         TIM_ICInitStructure.TIM_ICFilter = 0x00;
 365     }
 366
 367     TIM_ICInit(tim, &TIM_ICInitStructure);
 368 }
 369
 370 void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel)
 371 {
 372     pwmInputPort_t *self = &pwmInputPorts[channel];
 373
 374     self->state = 0;
 375     self->missedEvents = 0;
 376     self->channel = channel;
 377     self->mode = INPUT_MODE_PWM;
 378     self->timerHardware = timerHardwarePtr;
 379
 380     pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode);
 381     pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
 382
 383     timerConfigure(timerHardwarePtr, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_MHZ);
 384
 385     timerChCCHandlerInit(&self->edgeCb, pwmEdgeCallback);
 386     timerChOvrHandlerInit(&self->overflowCb, pwmOverflowCallback);
 387     timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb);
 388 }
 389
 390 #define UNUSED_PPM_TIMER_REFERENCE 0
 391 #define FIRST_PWM_PORT 0
 392
 393 void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer)
 394 {
 395     if (timerHardwarePtr->tim == sharedPwmTimer) {
 396         ppmCountShift = 3;  // Divide by 8 if the timer is running at 8 MHz
 397     }
 398 }
 399
 400 void ppmInConfig(const timerHardware_t *timerHardwarePtr)
 401 {
 402     ppmInit();
 403
 404     pwmInputPort_t *self = &pwmInputPorts[FIRST_PWM_PORT];
 405
 406     self->mode = INPUT_MODE_PPM;
 407     self->timerHardware = timerHardwarePtr;
 408
 409     pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode);
 410     pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
 411
 412     timerConfigure(timerHardwarePtr, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ);
 413
 414     timerChCCHandlerInit(&self->edgeCb, ppmEdgeCallback);
 415     timerChOvrHandlerInit(&self->overflowCb, ppmOverflowCallback);
 416     timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb);
 417 }
 418
 419 uint16_t ppmRead(uint8_t channel)
 420 {
 421     return captures[channel];
 422 }
 423
 424 uint16_t pwmRead(uint8_t channel)
 425 {
 426     return captures[channel];
 427 }