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.
This commit is contained in:
Dominic Clifton
parent
a5ec1355d4
commit
4b0fed8f07
|
|
@ -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) {
|
||||
ppmFrameCount++;
|
||||
}
|
||||
chan = 0;
|
||||
/* 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 {
|
||||
if (chan < PPM_CAPTURE_COUNT) {
|
||||
captures[chan] = diff;
|
||||
ppmDev.numChannels = ppmDev.pulseIndex;
|
||||
}
|
||||
chan++;
|
||||
} 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++;
|
||||
}
|
||||
|
||||
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]);
|
||||
|
|
|
|||
Loading…
Reference in New Issue