From 9738a1cdd2f0d28071f24e67cd3467d24c5698fd Mon Sep 17 00:00:00 2001 From: mikeller Date: Mon, 4 Jun 2018 22:41:54 +1200 Subject: [PATCH] Added more options for RC interpolation channel selection. --- src/main/fc/fc_rc.c | 81 +++++++++++++++++++++++++---------- src/main/fc/fc_rc.h | 8 ++++ src/main/interface/settings.c | 2 +- src/main/pg/rx.c | 3 +- 4 files changed, 70 insertions(+), 24 deletions(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index d38fb4b677..b4b9860710 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -59,6 +59,15 @@ static bool reverseMotors = false; static applyRatesFn *applyRates; uint16_t currentRxRefreshRate; +FAST_RAM_ZERO_INIT uint8_t interpolationChannels; + +enum { + ROLL_FLAG = 1 << ROLL, + PITCH_FLAG = 1 << PITCH, + YAW_FLAG = 1 << YAW, + THROTTLE_FLAG = 1 << THROTTLE, +}; + #ifdef USE_RC_SMOOTHING_FILTER #define RC_SMOOTHING_FILTER_TRAINING_DELAY_MS 3000 // Wait 3 seconds after power to let the PID loop stabilize before starting average frame rate calculation #define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50 @@ -197,7 +206,6 @@ FAST_CODE uint8_t processRcInterpolation(void) static FAST_RAM_ZERO_INIT float rcStepSize[4]; static FAST_RAM_ZERO_INIT int16_t rcInterpolationStepCount; - const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT" uint16_t rxRefreshRate; uint8_t updatedChannel = 0; @@ -219,8 +227,10 @@ FAST_CODE uint8_t processRcInterpolation(void) if (isRXDataNew && rxRefreshRate > 0) { rcInterpolationStepCount = rxRefreshRate / targetPidLooptime; - for (int channel = ROLL; channel < interpolationChannels; channel++) { - rcStepSize[channel] = (rcCommand[channel] - rcCommandInterp[channel]) / (float)rcInterpolationStepCount; + for (int channel = ROLL; channel <= THROTTLE; channel++) { + if ((1 << channel) & interpolationChannels) { + rcStepSize[channel] = (rcCommand[channel] - rcCommandInterp[channel]) / (float)rcInterpolationStepCount; + } } DEBUG_SET(DEBUG_RC_INTERPOLATION, 0, lrintf(rcCommand[0])); @@ -231,9 +241,11 @@ FAST_CODE uint8_t processRcInterpolation(void) // Interpolate steps of rcCommand if (rcInterpolationStepCount > 0) { - for (updatedChannel = ROLL; updatedChannel < interpolationChannels; updatedChannel++) { - rcCommandInterp[updatedChannel] += rcStepSize[updatedChannel]; - rcCommand[updatedChannel] = rcCommandInterp[updatedChannel]; + for (updatedChannel = ROLL; updatedChannel <= THROTTLE; updatedChannel++) { + if ((1 << updatedChannel) & interpolationChannels) { + rcCommandInterp[updatedChannel] += rcStepSize[updatedChannel]; + rcCommand[updatedChannel] = rcCommandInterp[updatedChannel]; + } } } } else { @@ -254,7 +266,6 @@ uint8_t calcRcSmoothingCutoff(float avgRxFrameRate, float filterBaseline) FAST_CODE uint8_t processRcSmoothingFilter(void) { - uint8_t updatedChannel = 0; static FAST_RAM_ZERO_INIT float lastRxData[4]; @@ -268,18 +279,18 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) static FAST_RAM_ZERO_INIT uint16_t defaultDerivativeCutoffFrequency; static FAST_RAM_ZERO_INIT uint16_t filterCutoffFrequency; static FAST_RAM_ZERO_INIT uint16_t derivativeCutoffFrequency; - static FAST_RAM_ZERO_INIT uint8_t interpolationChannels; if (!initialized) { initialized = true; - interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT" filterCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff; derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff; } if (isRXDataNew) { - for (int i = 0; i < interpolationChannels; i++) { - lastRxData[i] = rcCommand[i]; + for (int i = 0; i <= THROTTLE; i++) { + if ((1 << i) & interpolationChannels) { + lastRxData[i] = rcCommand[i]; + } } // If the filter cutoffs are set to auto and we have good rx data, then determine the average rx frame rate // and use that to calculate the filter cutoff frequencies @@ -295,15 +306,17 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) derivativeCutoffFrequency = (derivativeCutoffFrequency == 0) ? defaultDerivativeCutoffFrequency : derivativeCutoffFrequency; const float dT = targetPidLooptime * 1e-6f; - for (int i = 0; i < interpolationChannels; i++) { - switch (rxConfig()->rc_smoothing_input_type) { - case RC_SMOOTHING_INPUT_BIQUAD: - biquadFilterInitLPF(&rcCommandFilterBiquad[i], filterCutoffFrequency, targetPidLooptime); - break; - case RC_SMOOTHING_INPUT_PT1: - default: - pt1FilterInit(&rcCommandFilterPt1[i], pt1FilterGain(filterCutoffFrequency, dT)); - break; + for (int i = 0; i <= THROTTLE; i++) { + if ((1 << i) & interpolationChannels) { + switch (rxConfig()->rc_smoothing_input_type) { + case RC_SMOOTHING_INPUT_BIQUAD: + biquadFilterInitLPF(&rcCommandFilterBiquad[i], filterCutoffFrequency, targetPidLooptime); + break; + case RC_SMOOTHING_INPUT_PT1: + default: + pt1FilterInit(&rcCommandFilterPt1[i], pt1FilterGain(filterCutoffFrequency, dT)); + break; + } } } pidInitSetpointDerivativeLpf(derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type); @@ -319,8 +332,8 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rxConfig()->rc_smoothing_debug_axis])); DEBUG_SET(DEBUG_RC_SMOOTHING, 3, defaultInputCutoffFrequency); - for (updatedChannel = ROLL; updatedChannel < interpolationChannels; updatedChannel++) { - if (filterInitialized) { + for (updatedChannel = ROLL; updatedChannel <= THROTTLE; updatedChannel++) { + if (filterInitialized && (1 << updatedChannel) & interpolationChannels) { switch (rxConfig()->rc_smoothing_input_type) { case RC_SMOOTHING_INPUT_BIQUAD: rcCommand[updatedChannel] = biquadFilterApply(&rcCommandFilterBiquad[updatedChannel], lastRxData[updatedChannel]); @@ -517,4 +530,28 @@ void initRcProcessing(void) break; } + + interpolationChannels = 0; + switch (rxConfig()->rcInterpolationChannels) { + case INTERPOLATION_CHANNELS_RPYT: + interpolationChannels |= THROTTLE_FLAG; + + FALLTHROUGH; + case INTERPOLATION_CHANNELS_RPY: + interpolationChannels |= YAW_FLAG; + + FALLTHROUGH; + case INTERPOLATION_CHANNELS_RP: + interpolationChannels |= ROLL_FLAG | PITCH_FLAG; + + break; + case INTERPOLATION_CHANNELS_T: + interpolationChannels |= THROTTLE_FLAG; + + FALLTHROUGH; + case INTERPOLATION_CHANNELS_RPT: + interpolationChannels |= ROLL_FLAG | PITCH_FLAG; + + break; + } } diff --git a/src/main/fc/fc_rc.h b/src/main/fc/fc_rc.h index d2c1e264f9..3455f6824f 100644 --- a/src/main/fc/fc_rc.h +++ b/src/main/fc/fc_rc.h @@ -20,6 +20,14 @@ #pragma once +typedef enum { + INTERPOLATION_CHANNELS_RP, + INTERPOLATION_CHANNELS_RPY, + INTERPOLATION_CHANNELS_RPYT, + INTERPOLATION_CHANNELS_T, + INTERPOLATION_CHANNELS_RPT, +} interpolationChannels_e; + extern uint16_t currentRxRefreshRate; void processRcCommand(void); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 85897e6c33..d1e37ca5d6 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -259,7 +259,7 @@ static const char * const lookupTableRcInterpolation[] = { }; static const char * const lookupTableRcInterpolationChannels[] = { - "RP", "RPY", "RPYT" + "RP", "RPY", "RPYT", "T", "RPT", }; static const char * const lookupTableLowpassType[] = { diff --git a/src/main/pg/rx.c b/src/main/pg/rx.c index 75fb5dcece..5dcdadba4d 100644 --- a/src/main/pg/rx.c +++ b/src/main/pg/rx.c @@ -29,6 +29,7 @@ #include "config/config_reset.h" #include "drivers/io.h" +#include "fc/fc_rc.h" #include "fc/rc_controls.h" #include "rx/rx.h" #include "rx/rx_spi.h" @@ -55,7 +56,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig) .rssi_offset = 0, .rssi_invert = 0, .rcInterpolation = RC_SMOOTHING_AUTO, - .rcInterpolationChannels = 0, + .rcInterpolationChannels = INTERPOLATION_CHANNELS_RP, .rcInterpolationInterval = 19, .fpvCamAngleDegrees = 0, .airModeActivateThreshold = 32,