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_RX_PWM) || defined(USE_RX_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) % ARRAYLEN(ppmEvents);
 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     /* Store the current measurement */
 213     ppmDev.currentTime = currentTime;
 214     ppmDev.currentCapture = capture;
 215
 216     /* Sync pulse detection */
 217     if (ppmDev.deltaTime > PPM_IN_MIN_SYNC_PULSE_US) {
 218         if (ppmDev.pulseIndex == ppmDev.numChannelsPrevFrame
 219             && ppmDev.pulseIndex >= PPM_IN_MIN_NUM_CHANNELS
 220             && ppmDev.pulseIndex <= PPM_IN_MAX_NUM_CHANNELS) {
 221             /* If we see n simultaneous frames of the same
 222                number of channels we save it as our frame size */
 223             if (ppmDev.stableFramesSeenCount < PPM_STABLE_FRAMES_REQUIRED_COUNT) {
 224                 ppmDev.stableFramesSeenCount++;
 225             } else {
 226                 ppmDev.numChannels = ppmDev.pulseIndex;
 227             }
 228         } else {
 229             ppmDev.stableFramesSeenCount = 0;
 230         }
 231
 232         /* Check if the last frame was well formed */
 233         if (ppmDev.pulseIndex == ppmDev.numChannels && ppmDev.tracking) {
 234             /* The last frame was well formed */
 235             for (i = 0; i < ppmDev.numChannels; i++) {
 236                 captures[i] = ppmDev.captures[i];
 237             }
 238             for (i = ppmDev.numChannels; i < PPM_IN_MAX_NUM_CHANNELS; i++) {
 239                 captures[i] = PPM_RCVR_TIMEOUT;
 240             }
 241             ppmFrameCount++;
 242         }
 243
 244         ppmDev.tracking   = true;
 245         ppmDev.numChannelsPrevFrame = ppmDev.pulseIndex;
 246         ppmDev.pulseIndex = 0;
 247
 248         /* We rely on the supervisor to set captureValue to invalid
 249            if no valid frame is found otherwise we ride over it */
 250     } else if (ppmDev.tracking) {
 251         /* Valid pulse duration 0.75 to 2.5 ms*/
 252         if (ppmDev.deltaTime > PPM_IN_MIN_CHANNEL_PULSE_US
 253             && ppmDev.deltaTime < PPM_IN_MAX_CHANNEL_PULSE_US
 254             && ppmDev.pulseIndex < PPM_IN_MAX_NUM_CHANNELS) {
 255             ppmDev.captures[ppmDev.pulseIndex] = ppmDev.deltaTime;
 256             ppmDev.pulseIndex++;
 257         } else {
 258             /* Not a valid pulse duration */
 259             ppmDev.tracking = false;
 260             for (i = 0; i < PWM_PORTS_OR_PPM_CAPTURE_COUNT; i++) {
 261                 ppmDev.captures[i] = PPM_RCVR_TIMEOUT;
 262             }
 263         }
 264     }
 265 }
 266
 267 #define MAX_MISSED_PWM_EVENTS 10
 268
 269 bool isPWMDataBeingReceived(void)
 270 {
 271     int channel;
 272     for (channel = 0; channel < PWM_PORTS_OR_PPM_CAPTURE_COUNT; channel++) {
 273         if (captures[channel] != PPM_RCVR_TIMEOUT) {
 274             return true;
 275         }
 276     }
 277     return false;
 278 }
 279
 280 static void pwmOverflowCallback(timerOvrHandlerRec_t* cbRec, captureCompare_t capture)
 281 {
 282     UNUSED(capture);
 283     pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, overflowCb);
 284
 285     if (++pwmInputPort->missedEvents > MAX_MISSED_PWM_EVENTS) {
 286         captures[pwmInputPort->channel] = PPM_RCVR_TIMEOUT;
 287         pwmInputPort->missedEvents = 0;
 288     }
 289 }
 290
 291 static void pwmEdgeCallback(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
 292 {
 293     pwmInputPort_t *pwmInputPort = container_of(cbRec, pwmInputPort_t, edgeCb);
 294     const timerHardware_t *timerHardwarePtr = pwmInputPort->timerHardware;
 295
 296     if (pwmInputPort->state == 0) {
 297         pwmInputPort->rise = capture;
 298         pwmInputPort->state = 1;
 299 #if defined(USE_HAL_DRIVER)
 300         pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPOLARITY_FALLING);
 301 #else
 302         pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Falling);
 303 #endif
 304     } else {
 305         pwmInputPort->fall = capture;
 306
 307         // compute and store capture
 308         pwmInputPort->capture = pwmInputPort->fall - pwmInputPort->rise;
 309         captures[pwmInputPort->channel] = pwmInputPort->capture;
 310
 311         // switch state
 312         pwmInputPort->state = 0;
 313 #if defined(USE_HAL_DRIVER)
 314         pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPOLARITY_RISING);
 315 #else
 316         pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising);
 317 #endif
 318         pwmInputPort->missedEvents = 0;
 319     }
 320 }
 321
 322 #ifdef USE_HAL_DRIVER
 323
 324 void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
 325 {
 326     TIM_HandleTypeDef* Handle = timerFindTimerHandle(tim);
 327     if (Handle == NULL) return;
 328
 329     TIM_IC_InitTypeDef TIM_ICInitStructure;
 330
 331     TIM_ICInitStructure.ICPolarity = polarity;
 332     TIM_ICInitStructure.ICSelection = TIM_ICSELECTION_DIRECTTI;
 333     TIM_ICInitStructure.ICPrescaler = TIM_ICPSC_DIV1;
 334
 335     if (inputFilteringMode == INPUT_FILTERING_ENABLED) {
 336         TIM_ICInitStructure.ICFilter = INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX;
 337     } else {
 338         TIM_ICInitStructure.ICFilter = 0x00;
 339     }
 340
 341     HAL_TIM_IC_ConfigChannel(Handle, &TIM_ICInitStructure, channel);
 342     HAL_TIM_IC_Start_IT(Handle,channel);
 343 }
 344 #else
 345 void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity)
 346 {
 347     TIM_ICInitTypeDef TIM_ICInitStructure;
 348
 349     TIM_ICStructInit(&TIM_ICInitStructure);
 350     TIM_ICInitStructure.TIM_Channel = channel;
 351     TIM_ICInitStructure.TIM_ICPolarity = polarity;
 352     TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
 353     TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
 354
 355     if (inputFilteringMode == INPUT_FILTERING_ENABLED) {
 356         TIM_ICInitStructure.TIM_ICFilter = INPUT_FILTER_TO_HELP_WITH_NOISE_FROM_OPENLRS_TELEMETRY_RX;
 357     } else {
 358         TIM_ICInitStructure.TIM_ICFilter = 0x00;
 359     }
 360
 361     TIM_ICInit(tim, &TIM_ICInitStructure);
 362 }
 363 #endif
 364
 365 void pwmRxInit(const pwmConfig_t *pwmConfig)
 366 {
 367     inputFilteringMode = pwmConfig->inputFilteringMode;
 368
 369     for (int channel = 0; channel < PWM_INPUT_PORT_COUNT; channel++) {
 370
 371         pwmInputPort_t *port = &pwmInputPorts[channel];
 372
 373         const timerHardware_t *timer = timerAllocate(pwmConfig->ioTags[channel], OWNER_PWMINPUT, RESOURCE_INDEX(channel));
 374
 375         if (!timer) {
 376             /* TODO: maybe fail here if not enough channels? */
 377             continue;
 378         }
 379
 380         port->state = 0;
 381         port->missedEvents = 0;
 382         port->channel = channel;
 383         port->mode = INPUT_MODE_PWM;
 384         port->timerHardware = timer;
 385
 386         IO_t io = IOGetByTag(pwmConfig->ioTags[channel]);
 387         IOInit(io, OWNER_PWMINPUT, RESOURCE_INDEX(channel));
 388         IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction);
 389         timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_1MHZ);
 390         timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback);
 391         timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback);
 392         timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);
 393
 394 #if defined(USE_HAL_DRIVER)
 395         pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING);
 396 #else
 397         pwmICConfig(timer->tim, timer->channel, TIM_ICPolarity_Rising);
 398 #endif
 399
 400     }
 401 }
 402
 403 #define FIRST_PWM_PORT 0
 404
 405 #ifdef USE_PWM_OUTPUT
 406 void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer)
 407 {
 408     pwmOutputPort_t *motors = pwmGetMotors();
 409     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS; motorIndex++) {
 410         if (!motors[motorIndex].enabled || motors[motorIndex].channel.tim != pwmTimer) {
 411             continue;
 412         }
 413
 414         ppmCountDivisor = timerClock(pwmTimer) / (pwmTimer->PSC + 1);
 415         return;
 416     }
 417 }
 418 #endif
 419
 420 void ppmRxInit(const ppmConfig_t *ppmConfig)
 421 {
 422     ppmResetDevice();
 423
 424     pwmInputPort_t *port = &pwmInputPorts[FIRST_PWM_PORT];
 425
 426     const timerHardware_t *timer = timerAllocate(ppmConfig->ioTag, OWNER_PPMINPUT, 0);
 427     if (!timer) {
 428         /* TODO: fail here? */
 429         return;
 430     }
 431
 432 #ifdef USE_PWM_OUTPUT
 433     ppmAvoidPWMTimerClash(timer->tim);
 434 #endif
 435
 436     port->mode = INPUT_MODE_PPM;
 437     port->timerHardware = timer;
 438
 439     IO_t io = IOGetByTag(ppmConfig->ioTag);
 440     IOInit(io, OWNER_PPMINPUT, 0);
 441     IOConfigGPIOAF(io, IOCFG_AF_PP, timer->alternateFunction);
 442
 443     timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_1MHZ);
 444     timerChCCHandlerInit(&port->edgeCb, ppmEdgeCallback);
 445     timerChOvrHandlerInit(&port->overflowCb, ppmOverflowCallback);
 446     timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb);
 447
 448 #if defined(USE_HAL_DRIVER)
 449     pwmICConfig(timer->tim, timer->channel, TIM_ICPOLARITY_RISING);
 450 #else
 451     pwmICConfig(timer->tim, timer->channel, TIM_ICPolarity_Rising);
 452 #endif
 453 }
 454
 455 uint16_t ppmRead(uint8_t channel)
 456 {
 457     return captures[channel];
 458 }
 459
 460 uint16_t pwmRead(uint8_t channel)
 461 {
 462     return captures[channel];
 463 }
 464 #endif