From 4b0fed8f075e5a262cd708350160de4ef7f86a80 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 26 Jul 2014 11:59:03 +0100 Subject: [PATCH] Ported OpenPilot PPM frame validation. On the CC3D it was observed that jitters were present, this code validates each frame. Prior to this, on other boards (Naze32/Flip32+/Olimexino/etc) it was observed that when using an 8 channel RX AUX5-8 would sometimes be set due to bad PPM data. This code fixes both issues. --- src/main/drivers/pwm_rx.c | 125 +++++++++++++++++++++++++++++++------- 1 file changed, 102 insertions(+), 23 deletions(-) diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 3802ff5578..5d10c6555c 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -67,6 +67,32 @@ static uint16_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT]; static uint8_t ppmFrameCount = 0; static uint8_t lastPPMFrameCount = 0; +typedef struct ppmDevice { + uint8_t pulseIndex; + uint32_t previousTime; + uint32_t currentTime; + uint32_t deltaTime; + uint32_t captures[PWM_PORTS_OR_PPM_CAPTURE_COUNT]; + uint32_t largeCounter; + int8_t numChannels; + int8_t numChannelsPrevFrame; + uint8_t stableFramesSeenCount; + + bool tracking; +} ppmDevice_t; + +ppmDevice_t ppmDev; + + +#define PPM_IN_MIN_SYNC_PULSE_US 2700 // microseconds +#define PPM_IN_MIN_CHANNEL_PULSE_US 750 // microseconds +#define PPM_IN_MAX_CHANNEL_PULSE_US 2250 // microseconds +#define PPM_STABLE_FRAMES_REQUIRED_COUNT 25 +#define PPM_IN_MIN_NUM_CHANNELS 4 +#define PPM_IN_MAX_NUM_CHANNELS PWM_PORTS_OR_PPM_CAPTURE_COUNT +#define PPM_RCVR_TIMEOUT 0 + + bool isPPMDataBeingReceived(void) { return (ppmFrameCount != lastPPMFrameCount); @@ -79,48 +105,99 @@ void resetPPMDataReceivedState(void) #define MIN_CHANNELS_BEFORE_PPM_FRAME_CONSIDERED_VALID 4 -uint32_t largeCounter = 0; +static void ppmInit(void) +{ + ppmDev.pulseIndex = 0; + ppmDev.previousTime = 0; + ppmDev.currentTime = 0; + ppmDev.deltaTime = 0; + ppmDev.largeCounter = 0; + ppmDev.numChannels = -1; + ppmDev.numChannelsPrevFrame = -1; + ppmDev.stableFramesSeenCount = 0; + ppmDev.tracking = false; +} static void ppmOverflowCallback(uint8_t port, captureCompare_t capture) { - largeCounter += capture; + ppmDev.largeCounter += capture; } static void ppmEdgeCallback(uint8_t port, captureCompare_t capture) { - uint32_t diff; // See PPM_TIMER_PERIOD - static uint32_t now = 0; - static uint32_t last = 0; + int32_t i; - static uint8_t chan = 0; + /* Shift the last measurement out */ + ppmDev.previousTime = ppmDev.currentTime; - last = now; - now = capture; + /* Grab the new count */ + ppmDev.currentTime = capture; - now += largeCounter; + /* Convert to 32-bit timer result */ + ppmDev.currentTime += ppmDev.largeCounter; - diff = now - last; + /* Capture computation */ + ppmDev.deltaTime = ppmDev.currentTime - ppmDev.previousTime; + + ppmDev.previousTime = ppmDev.currentTime; #if 0 - static uint32_t diffs[20]; - static uint8_t diffIndex = 0; + static uint32_t deltaTimes[20]; + static uint8_t deltaIndex = 0; - diffIndex = (diffIndex + 1) % 20; - diffs[diffIndex] = diff; + deltaIndex = (deltaIndex + 1) % 20; + deltaTimes[deltaIndex] = ppmDev.deltaTime; #endif - if (diff > 2700) { // Per http://www.rcgroups.com/forums/showpost.php?p=21996147&postcount=3960 "So, if you use 2.5ms or higher as being the reset for the PPM stream start, you will be fine. I use 2.7ms just to be safe." - if (chan >= MIN_CHANNELS_BEFORE_PPM_FRAME_CONSIDERED_VALID) { + /* Sync pulse detection */ + if (ppmDev.deltaTime > PPM_IN_MIN_SYNC_PULSE_US) { + if (ppmDev.pulseIndex == ppmDev.numChannelsPrevFrame + && ppmDev.pulseIndex >= PPM_IN_MIN_NUM_CHANNELS + && ppmDev.pulseIndex <= PPM_IN_MAX_NUM_CHANNELS) { + /* If we see n simultaneous frames of the same + number of channels we save it as our frame size */ + if (ppmDev.stableFramesSeenCount < PPM_STABLE_FRAMES_REQUIRED_COUNT) { + ppmDev.stableFramesSeenCount++; + } else { + ppmDev.numChannels = ppmDev.pulseIndex; + } + } else { + ppmDev.stableFramesSeenCount = 0; + } + + /* Check if the last frame was well formed */ + if (ppmDev.pulseIndex == ppmDev.numChannels && ppmDev.tracking) { + /* The last frame was well formed */ + for (i = 0; i < ppmDev.numChannels; i++) { + captures[i] = ppmDev.captures[i]; + } + for (i = ppmDev.numChannels; i < PPM_IN_MAX_NUM_CHANNELS; i++) { + captures[i] = PPM_RCVR_TIMEOUT; + } ppmFrameCount++; } - chan = 0; - } else { - if (chan < PPM_CAPTURE_COUNT) { - captures[chan] = diff; - } - chan++; - } + ppmDev.tracking = true; + ppmDev.numChannelsPrevFrame = ppmDev.pulseIndex; + ppmDev.pulseIndex = 0; + + /* We rely on the supervisor to set captureValue to invalid + if no valid frame is found otherwise we ride over it */ + } else if (ppmDev.tracking) { + /* Valid pulse duration 0.75 to 2.5 ms*/ + if (ppmDev.deltaTime > PPM_IN_MIN_CHANNEL_PULSE_US + && ppmDev.deltaTime < PPM_IN_MAX_CHANNEL_PULSE_US + && ppmDev.pulseIndex < PPM_IN_MAX_NUM_CHANNELS) { + ppmDev.captures[ppmDev.pulseIndex] = ppmDev.deltaTime; + ppmDev.pulseIndex++; + } else { + /* Not a valid pulse duration */ + ppmDev.tracking = false; + for (i = 0; i < PWM_PORTS_OR_PPM_CAPTURE_COUNT; i++) { + ppmDev.captures[i] = PPM_RCVR_TIMEOUT; + } + } + } } static void pwmEdgeCallback(uint8_t port, captureCompare_t capture) @@ -193,6 +270,8 @@ void pwmInConfig(uint8_t timerIndex, uint8_t channel) void ppmInConfig(uint8_t timerIndex) { + ppmInit(); + pwmInputPort_t *p = &pwmInputPorts[FIRST_PWM_PORT]; const timerHardware_t *timerHardwarePtr = &(timerHardware[timerIndex]);