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
  26 #include "common/utils.h"
  27
  28 #include "system.h"
  29
  30 #include "nvic.h"
  31 #include "gpio.h"
  32 #include "timer.h"
  33
  34 #include "pwm_mapping.h"
  35
  36 #include "pwm_rx.h"
  37
  38 #define PPM_CAPTURE_COUNT 12
  39 #define PWM_INPUT_PORT_COUNT 8
  40
  41 #if PPM_CAPTURE_COUNT > MAX_PWM_INPUT_PORTS
  42 #define PWM_PORTS_OR_PPM_CAPTURE_COUNT PPM_CAPTURE_COUNT
  43 #else
  44 #define PWM_PORTS_OR_PPM_CAPTURE_COUNT PWM_INPUT_PORT_COUNT
  45 #endif
  46
  47 // TODO - change to timer cloks ticks
  48 #define INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX 0x03
  49
  50 static inputFilteringMode_e inputFilteringMode;
  51
  52 void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity);
  53
  54 typedef enum {
  55     INPUT_MODE_PPM,
  56     INPUT_MODE_PWM,
  57 } pwmInputMode_t;
  58
  59 typedef struct {
  60     pwmInputMode_t mode;
  61     uint8_t channel; // only used for pwm, ignored by ppm
  62
  63     uint8_t state;
  64     captureCompare_t rise;
  65     captureCompare_t fall;
  66     captureCompare_t capture;
  67
  68     uint8_t missedEvents;
  69
  70     const timerHardware_t *timerHardware;
  71     timerCCHandlerRec_t edgeCb;
  72     timerOvrHandlerRec_t overflowCb;
  73 } pwmInputPort_t;
  74
  75 static pwmInputPort_t pwmInputPorts[PWM_INPUT_PORT_COUNT];
  76
  77 static uint16_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT];
  78
  79 #define PPM_TIMER_PERIOD 0x10000
  80 #define PWM_TIMER_PERIOD 0x10000
  81
  82 static uint8_t ppmFrameCount = 0;
  83 static uint8_t lastPPMFrameCount = 0;
  84 static uint8_t ppmCountShift = 0;
  85
  86 typedef struct ppmDevice {
  87     uint8_t  pulseIndex;
  88     uint32_t previousTime;
  89     uint32_t currentTime;
  90     uint32_t deltaTime;
  91     uint32_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT];
  92     uint32_t largeCounter;
  93     int8_t   numChannels;
  94     int8_t   numChannelsPrevFrame;
  95     uint8_t  stableFramesSeenCount;
  96
  97     bool     tracking;
  98 } ppmDevice_t;
  99
 100 ppmDevice_t ppmDev;
 101
 102
 103 #define PPM_IN_MIN_SYNC_PULSE_US    2700    // microseconds
 104 #define PPM_IN_MIN_CHANNEL_PULSE_US 750     // microseconds
 105 #define PPM_IN_MAX_CHANNEL_PULSE_US 2250    // microseconds
 106 #define PPM_STABLE_FRAMES_REQUIRED_COUNT    25
 107 #define PPM_IN_MIN_NUM_CHANNELS     4
 108 #define PPM_IN_MAX_NUM_CHANNELS     PWM_PORTS_OR_PPM_CAPTURE_COUNT
 109
 110
 111 bool isPPMDataBeingReceived(void)
 112 {
 113     return (ppmFrameCount != lastPPMFrameCount);
 114 }
 115
 116 void resetPPMDataReceivedState(void)
 117 {
 118     lastPPMFrameCount = ppmFrameCount;
 119 }
 120
 121 #define MIN_CHANNELS_BEFORE_PPM_FRAME_CONSIDERED_VALID 4
 122
 123 void pwmRxInit(inputFilteringMode_e initialInputFilteringMode)
 124 {
 125     inputFilteringMode = initialInputFilteringMode;
 126 }
 127
 128 static void ppmInit(void)
 129 {
 130     ppmDev.pulseIndex   = 0;
 131     ppmDev.previousTime = 0;
 132     ppmDev.currentTime  = 0;
 133     ppmDev.deltaTime    = 0;
 134     ppmDev.largeCounter = 0;
 135     ppmDev.numChannels  = -1;
 136     ppmDev.numChannelsPrevFrame = -1;
 137     ppmDev.stableFramesSeenCount = 0;
 138     ppmDev.tracking     = false;
 139 }
 140
 141 static void ppmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture)
 142 {
 143     UNUSED(cbRec);
 144     ppmDev.largeCounter += capture + 1;
 145 }
 146
 147 static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture)
 148 {
 149     UNUSED(cbRec);
 150     int32_t i;
 151
 152     /* Shift the last measurement out */
 153     ppmDev.previousTime = ppmDev.currentTime;
 154
 155     /* Grab the new count */
 156     ppmDev.currentTime  = capture;
 157
 158     /* Convert to 32-bit timer result */
 159     ppmDev.currentTime += ppmDev.largeCounter;
 160
 161     // Divide by 8 if Oneshot125 is active and this is a CC3D board
 162     ppmDev.currentTime = ppmDev.currentTime >> ppmCountShift;
 163
 164     /* Capture computation */
 165     ppmDev.deltaTime    = ppmDev.currentTime - ppmDev.previousTime;
 166
 167     ppmDev.previousTime = ppmDev.currentTime;
 168
 169 #if 0
 170     static uint32_t deltaTimes[20];
 171     static uint8_t deltaIndex = 0;
 172
 173     deltaIndex = (deltaIndex + 1) % 20;
 174     deltaTimes[deltaIndex] = ppmDev.deltaTime;
 175 #endif
 176
 177     /* Sync pulse detection */
 178     if (ppmDev.deltaTime > PPM_IN_MIN_SYNC_PULSE_US) {
 179         if (ppmDev.pulseIndex == ppmDev.numChannelsPrevFrame
 180             && ppmDev.pulseIndex >= PPM_IN_MIN_NUM_CHANNELS
 181             && ppmDev.pulseIndex <= PPM_IN_MAX_NUM_CHANNELS) {
 182             /* If we see n simultaneous frames of the same
 183                number of channels we save it as our frame size */
 184             if (ppmDev.stableFramesSeenCount < PPM_STABLE_FRAMES_REQUIRED_COUNT) {
 185                 ppmDev.stableFramesSeenCount++;
 186             } else {
 187                 ppmDev.numChannels = ppmDev.pulseIndex;
 188             }
 189         } else {
 190             ppmDev.stableFramesSeenCount = 0;
 191         }
 192
 193         /* Check if the last frame was well formed */
 194         if (ppmDev.pulseIndex == ppmDev.numChannels && ppmDev.tracking) {
 195             /* The last frame was well formed */
 196             for (i = 0; i < ppmDev.numChannels; i++) {
 197                 captures[i] = ppmDev.captures[i];
 198             }
 199             for (i = ppmDev.numChannels; i < PPM_IN_MAX_NUM_CHANNELS; i++) {
 200                 captures[i] = PPM_RCVR_TIMEOUT;
 201             }
 202             ppmFrameCount++;
 203         }
 204
 205         ppmDev.tracking   = true;
 206         ppmDev.numChannelsPrevFrame = ppmDev.pulseIndex;
 207         ppmDev.pulseIndex = 0;
 208
 209         /* We rely on the supervisor to set captureValue to invalid
 210            if no valid frame is found otherwise we ride over it */
 211     } else if (ppmDev.tracking) {
 212         /* Valid pulse duration 0.75 to 2.5 ms*/
 213         if (ppmDev.deltaTime > PPM_IN_MIN_CHANNEL_PULSE_US
 214             && ppmDev.deltaTime < PPM_IN_MAX_CHANNEL_PULSE_US
 215             && ppmDev.pulseIndex < PPM_IN_MAX_NUM_CHANNELS) {
 216             ppmDev.captures[ppmDev.pulseIndex] = ppmDev.deltaTime;
 217             ppmDev.pulseIndex++;
 218         } else {
 219             /* Not a valid pulse duration */
 220             ppmDev.tracking = false;
 221             for (i = 0; i < PWM_PORTS_OR_PPM_CAPTURE_COUNT; i++) {
 222                 ppmDev.captures[i] = PPM_RCVR_TIMEOUT;
 223             }
 224         }
 225     }
 226 }
 227
 228 #define MAX_MISSED_PWM_EVENTS 10
 229
 230 bool isPWMDataBeingReceived(void)
 231 {
 232     int channel;
 233     for (channel = 0; channel < PWM_PORTS_OR_PPM_CAPTURE_COUNT; channel++) {
 234         if (captures[channel] != PPM_RCVR_TIMEOUT) {
 235             return true;
 236         }
 237     }
 238     return false;
 239 }
 240
 241 static void pwmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture)
 242 {
 243     UNUSED(capture);
 244     pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, overflowCb);
 245
 246     if (++pwmInputPort->missedEvents > MAX_MISSED_PWM_EVENTS) {
 247         captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT;
 248         pwmInputPort->missedEvents = 0;
 249     }
 250 }
 251
 252 static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
 253 {
 254     pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, edgeCb);
 255     const timerHardware_t *timerHardwarePtr = pwmInputPort->timerHardware;
 256
 257     if (pwmInputPort->state == 0) {
 258         pwmInputPort->rise = capture;
 259         pwmInputPort->state = 1;
 260         pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
 261     } else {
 262         pwmInputPort->fall = capture;
 263
 264         // compute and store capture
 265         pwmInputPort->capture = pwmInputPort->fall - pwmInputPort->rise;
 266         captures[pwmInputPort->channel] = pwmInputPort->capture;
 267
 268         // switch state
 269         pwmInputPort->state = 0;
 270         pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
 271         pwmInputPort->missedEvents = 0;
 272     }
 273 }
 274
 275 static void pwmGPIOConfig(GPIO_TypeDef *gpio, uint32_t pin, GPIO_Mode mode)
 276 {
 277     gpio_config_t cfg;
 278
 279     cfg.pin = pin;
 280     cfg.mode = mode;
 281     cfg.speed = Speed_2MHz;
 282     gpioInit(gpio, &cfg);
 283 }
 284
 285 void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
 286 {
 287     TIM_ICInitTypeDef TIM_ICInitStructure;
 288
 289     TIM_ICStructInit(&TIM_ICInitStructure);
 290     TIM_ICInitStructure.TIM_Channel = channel;
 291     TIM_ICInitStructure.TIM_ICPolarity = polarity;
 292     TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
 293     TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
 294
 295     if (inputFilteringMode == INPUT_FILTERING_ENABLED) {
 296         TIM_ICInitStructure.TIM_ICFilter = INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX;
 297     } else {
 298         TIM_ICInitStructure.TIM_ICFilter = 0x00;
 299     }
 300
 301     TIM_ICInit(tim, &TIM_ICInitStructure);
 302 }
 303
 304 void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel)
 305 {
 306     pwmInputPort_t *self = &pwmInputPorts[channel];
 307
 308     self->state = 0;
 309     self->missedEvents = 0;
 310     self->channel = channel;
 311     self->mode = INPUT_MODE_PWM;
 312     self->timerHardware = timerHardwarePtr;
 313
 314     pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode);
 315     pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
 316
 317     timerConfigure(timerHardwarePtr, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_MHZ);
 318
 319     timerChCCHandlerInit(&self->edgeCb, pwmEdgeCallback);
 320     timerChOvrHandlerInit(&self->overflowCb, pwmOverflowCallback);
 321     timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb);
 322 }
 323
 324 #define UNUSED_PPM_TIMER_REFERENCE 0
 325 #define FIRST_PWM_PORT 0
 326
 327 void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer)
 328 {
 329     if (timerHardwarePtr->tim == sharedPwmTimer) {
 330         ppmCountShift = 3;  // Divide by 8 if the timer is running at 8 MHz
 331     }
 332 }
 333
 334 void ppmInConfig(const timerHardware_t *timerHardwarePtr)
 335 {
 336     ppmInit();
 337
 338     pwmInputPort_t *self = &pwmInputPorts[FIRST_PWM_PORT];
 339
 340     self->mode = INPUT_MODE_PPM;
 341     self->timerHardware = timerHardwarePtr;
 342
 343     pwmGPIOConfig(timerHardwarePtr->gpio, timerHardwarePtr->pin, timerHardwarePtr->gpioInputMode);
 344     pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
 345
 346     timerConfigure(timerHardwarePtr, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ);
 347
 348     timerChCCHandlerInit(&self->edgeCb, ppmEdgeCallback);
 349     timerChOvrHandlerInit(&self->overflowCb, ppmOverflowCallback);
 350     timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb);
 351 }
 352
 353 uint16_t ppmRead(uint8_t channel)
 354 {
 355     return captures[channel];
 356 }
 357
 358 uint16_t pwmRead(uint8_t channel)
 359 {
 360     return captures[channel];
 361 }