From ff23cab2a8e30d4b90acb8de3558b6bbf0775d1c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 20 Aug 2015 21:59:07 +0200 Subject: [PATCH] Remove Aux filter // more accurate cycleTime // Code cleanup Change datatypes --- src/main/mw.c | 97 ++++++------------------------------------------ src/main/rx/rx.c | 13 +++++++ src/main/rx/rx.h | 1 + 3 files changed, 26 insertions(+), 85 deletions(-) diff --git a/src/main/mw.c b/src/main/mw.c index 5fc1e4159..a826988ad 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -93,8 +93,6 @@ 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 @@ -109,7 +107,7 @@ 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; +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 @@ -685,92 +683,28 @@ 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; + static int16_t factor, rcInterpolationFactor; + static filterStatePt1_t filteredCycleTimeState; + uint16_t rxRefreshRate, filteredCycleTime; // Set RC refresh rate for sampling and channels to filter - if (!rxRefreshRate) { - if (feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) { - rxRefreshRate = 20000; + initRxRefreshRate(&rxRefreshRate); - // AUX Channels to filter to replace PPM/PWM averaging - getArmingChannel(currentProfile->modeActivationConditions,&auxChannelToFilter); + filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1); + rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1; - } - - // 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) { + 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; + isRXDataNew = false; factor = rcInterpolationFactor - 1; - } - - else { + } else { factor--; } @@ -779,14 +713,7 @@ void filterRc(void){ 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 { + } else { factor = 0; } } @@ -802,7 +729,7 @@ void loop(void) if (shouldProcessRx(currentTime)) { processRx(); - isRXdataNew = true; + isRXDataNew = true; #ifdef BARO // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data. diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index c252feb77..7fbfdf99e 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -91,6 +91,7 @@ static uint16_t nullReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channe } static rcReadRawDataPtr rcReadRawFunc = nullReadRawRC; +static uint16_t rxRefreshRate; void serialRxInit(rxConfig_t *rxConfig); @@ -161,6 +162,7 @@ void rxInit(rxConfig_t *rxConfig) } if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) { + rxRefreshRate = 20000; rxPwmInit(&rxRuntimeConfig, &rcReadRawFunc); } @@ -173,20 +175,28 @@ void serialRxInit(rxConfig_t *rxConfig) bool enabled = false; switch (rxConfig->serialrx_provider) { case SERIALRX_SPEKTRUM1024: + rxRefreshRate = 22000; + enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); + break; case SERIALRX_SPEKTRUM2048: + rxRefreshRate = 11000; enabled = spektrumInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_SBUS: + rxRefreshRate = 11000; enabled = sbusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_SUMD: + rxRefreshRate = 11000; enabled = sumdInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_SUMH: + rxRefreshRate = 11000; enabled = sumhInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; case SERIALRX_XBUS_MODE_B: case SERIALRX_XBUS_MODE_B_RJ01: + rxRefreshRate = 11000; enabled = xBusInit(rxConfig, &rxRuntimeConfig, &rcReadRawFunc); break; } @@ -545,4 +555,7 @@ void updateRSSI(uint32_t currentTime) } } +void initRxRefreshRate(uint16_t *rxRefreshRatePtr) { + *rxRefreshRatePtr = rxRefreshRate; +} diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 058c3b089..31a8f5628 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -143,3 +143,4 @@ uint8_t serialRxFrameStatus(rxConfig_t *rxConfig); void updateRSSI(uint32_t currentTime); void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChannelRangeConfiguration); +void initRxRefreshRate(uint16_t *rxRefreshRatePtr);