src/main/drivers/rx/rx_pwm.c

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