From eaff64d25f56c2041226bccc51cbb8053bc6da7c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 15 Jul 2015 17:23:57 +0200 Subject: [PATCH] RC Smoothing fix loopcount Improve AUX logic // initial factor set to 1 Better Delta approach FIX AUX CALC Update refresh rates --- src/main/mw.c | 113 +++++++++++++++++++++++++++++++++++++++++++++++ src/main/rx/rx.c | 2 +- 2 files changed, 114 insertions(+), 1 deletion(-) diff --git a/src/main/mw.c b/src/main/mw.c index 5a376f8f23..708dc006ab 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -93,6 +93,8 @@ enum { /* IBat monitoring interval (in microseconds) - 6 default looptimes */ #define IBATINTERVAL (6 * 3500) +#define LOOP_DEADBAND 400 // Dead band for loop to modify to rcInterpolationFactor in RC Filtering for unstable looptimes + uint32_t currentTime = 0; uint32_t previousTime = 0; uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop @@ -107,6 +109,8 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3]; +static bool isRXdataNew; + typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); // pid controller function prototype @@ -681,6 +685,112 @@ void processRx(void) } +void getArmingChannel(modeActivationCondition_t *modeActivationConditions, uint8_t *armingChannel) { + for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { + modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; + if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) { + *armingChannel = modeActivationCondition->auxChannelIndex + NON_AUX_CHANNEL_COUNT; + break; + } + } +} + +void filterRc(void){ + static int16_t lastCommand[4] = { 0, 0, 0, 0 }; + static int16_t deltaRC[4] = { 0, 0, 0, 0 }; + static int16_t loop[5] = { 0, 0, 0, 0, 0 }; + static int16_t factor, rcInterpolationFactor, loopAvg; + static uint32_t rxRefreshRate; + static int16_t lastAux, deltaAux; // last arming AUX position and delta for arming AUX + static uint8_t auxChannelToFilter; // AUX channel used for arming needs filtering when used + static int loopCount; + + // Set RC refresh rate for sampling and channels to filter + if (!rxRefreshRate) { + if (feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) { + rxRefreshRate = 20000; + + // AUX Channels to filter to replace PPM/PWM averaging + getArmingChannel(currentProfile->modeActivationConditions,&auxChannelToFilter); + + } + + // TODO Are there more different refresh rates? + else { + switch (masterConfig.rxConfig.serialrx_provider) { + case SERIALRX_SPEKTRUM1024: + rxRefreshRate = 22000; + break; + case SERIALRX_SPEKTRUM2048: + rxRefreshRate = 11000; + break; + case SERIALRX_SBUS: + rxRefreshRate = 11000; + break; + default: + rxRefreshRate = 10000; + break; + } + } + + rcInterpolationFactor = 1; // Initial Factor before looptime average is calculated + + } + + // Averaging of cycleTime for more precise sampling + loop[loopCount] = cycleTime; + loopCount++; + + // Start recalculating new rcInterpolationFactor every 5 loop iterations + if (loopCount > 4) { + uint16_t tmp = (loop[0] + loop[1] + loop[2] + loop[3] + loop[4]) / 5; + + // Jitter tolerance to prevent rcInterpolationFactor jump too much + if (tmp > (loopAvg + LOOP_DEADBAND) || tmp < (loopAvg - LOOP_DEADBAND)) { + loopAvg = tmp; + rcInterpolationFactor = rxRefreshRate / loopAvg + 1; + } + + loopCount = 0; + } + + if (isRXdataNew) { + for (int channel=0; channel < 4; channel++) { + deltaRC[channel] = rcData[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor); + lastCommand[channel] = rcData[channel]; + } + + // Read AUX channel (arm/disarm guard enhancement) + if (auxChannelToFilter) { + deltaAux = rcData[auxChannelToFilter] - (lastAux - deltaAux * factor/rcInterpolationFactor); + lastAux = rcData[auxChannelToFilter]; + } + + isRXdataNew = false; + factor = rcInterpolationFactor - 1; + } + + else { + factor--; + } + + // Interpolate steps of rcData + if (factor > 0) { + for (int channel=0; channel < 4; channel++) { + rcData[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor; + } + + // Interpolate steps of Aux + if (auxChannelToFilter) { + rcData[auxChannelToFilter] = lastAux - deltaAux * factor/rcInterpolationFactor; + } + } + + else { + factor = 0; + } +} + void loop(void) { static uint32_t loopTime; @@ -692,6 +802,7 @@ void loop(void) if (shouldProcessRx(currentTime)) { processRx(); + isRXdataNew = true; #ifdef BARO // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data. @@ -746,6 +857,8 @@ void loop(void) } } + filterRc(); + annexCode(); #if defined(BARO) || defined(SONAR) haveProcessedAnnexCodeOnce = true; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 78016c43bb..c252feb77f 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -75,7 +75,7 @@ static uint32_t needRxSignalBefore = 0; int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] -#define PPM_AND_PWM_SAMPLE_COUNT 4 +#define PPM_AND_PWM_SAMPLE_COUNT 3 #define DELAY_50_HZ (1000000 / 50) #define DELAY_10_HZ (1000000 / 10)