From eaff64d25f56c2041226bccc51cbb8053bc6da7c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 15 Jul 2015 17:23:57 +0200 Subject: [PATCH 1/4] 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 5a376f8f2..708dc006a 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 78016c43b..c252feb77 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) From b69bab33b2d04b933674f5f126b51e3a978542db Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Sun, 16 Aug 2015 23:25:16 +0200 Subject: [PATCH 2/4] Configurable RC Smoothing --- src/main/config/config.c | 1 + src/main/io/serial_cli.c | 1 + src/main/mw.c | 4 +++- src/main/rx/rx.h | 1 + 4 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 6764ad4ab..3f615ecf5 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -414,6 +414,7 @@ static void resetConf(void) masterConfig.rxConfig.rssi_channel = 0; masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT; masterConfig.rxConfig.rssi_ppm_invert = 0; + masterConfig.rxConfig.rcSmoothing = 1; resetAllRxChannelRangeConfigurations(masterConfig.rxConfig.channelRanges); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index eddbf960a..fbc6ca197 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -316,6 +316,7 @@ const clivalue_t valueTable[] = { { "rssi_channel", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_channel, 0, MAX_SUPPORTED_RC_CHANNEL_COUNT }, { "rssi_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_scale, RSSI_SCALE_MIN, RSSI_SCALE_MAX }, { "rssi_ppm_invert", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rssi_ppm_invert, 0, 1 }, + { "rc_smoothing", VAR_INT8 | MASTER_VALUE, &masterConfig.rxConfig.rcSmoothing, 0, 1 }, { "input_filtering_mode", VAR_INT8 | MASTER_VALUE, &masterConfig.inputFilteringMode, 0, 1 }, { "min_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.minthrottle, PWM_RANGE_ZERO, PWM_RANGE_MAX }, diff --git a/src/main/mw.c b/src/main/mw.c index 708dc006a..5fc1e4159 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -857,7 +857,9 @@ void loop(void) } } - filterRc(); + if (masterConfig.rxConfig.rcSmoothing) { + filterRc(); + } annexCode(); #if defined(BARO) || defined(SONAR) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 7ab45f106..058c3b089 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -106,6 +106,7 @@ typedef struct rxConfig_s { uint8_t rssi_channel; uint8_t rssi_scale; uint8_t rssi_ppm_invert; + uint8_t rcSmoothing; // Enable/Disable RC filtering uint16_t midrc; // Some radios have not a neutral point centered on 1500. can be changed here uint16_t mincheck; // minimum rc end uint16_t maxcheck; // maximum rc end From ff23cab2a8e30d4b90acb8de3558b6bbf0775d1c Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 20 Aug 2015 21:59:07 +0200 Subject: [PATCH 3/4] 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); From 656cfb07742a9adfe6916478fca10ad1411aa62a Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Thu, 20 Aug 2015 22:24:29 +0200 Subject: [PATCH 4/4] cli doc for rc_smoothing --- docs/Cli.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/Cli.md b/docs/Cli.md index b8b9c9880..e25191add 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -111,6 +111,7 @@ Re-apply any new defaults as desired. | `rssi_scale` | | 1 | 255 | 30 | Master | UINT8 | | `rssi_ppm_invert` | | 0 | 1 | 0 | Master | UINT8 | | `input_filtering_mode` | | 0 | 1 | 0 | Master | INT8 | +| `rc_smoothing ` | Interpolation of Rc data during looptimes when there are no new updates. This gives smoother RC input to PID controller and cleaner PIDsum | 0 | 1 | 1 | Master | INT8 | | `min_throttle` | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. | 0 | 2000 | 1150 | Master | UINT16 | | `max_throttle` | These are min/max values (in us) that are sent to esc when armed. Defaults of 1150/1850 are OK for everyone, for use with AfroESC, they could be set to 1064/1864. | 0 | 2000 | 1850 | Master | UINT16 | | `min_command` | This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly when powered up, try decreasing this value. It can also be used for calibrating all ESCs at once. | 0 | 2000 | 1000 | Master | UINT16 |