diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 20444b0f34..506892c97b 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1225,6 +1225,10 @@ static bool blackboxWriteSysinfo(void) char buf[FORMATTED_DATE_TIME_BUFSIZE]; +#ifdef USE_RC_SMOOTHING_FILTER + rcSmoothingFilter_t *rcSmoothingData = getRcSmoothingData(); +#endif + const controlRateConfig_t *currentControlRateProfile = controlRateProfiles(systemConfig()->activeRateProfile); switch (xmitState.headerIndex) { BLACKBOX_PRINT_HEADER_LINE("Firmware type", "%s", "Cleanflight"); @@ -1377,16 +1381,16 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("features", "%d", featureConfig()->enabledFeatures); #ifdef USE_RC_SMOOTHING_FILTER - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_type", "%d", rxConfig()->rc_smoothing_type); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_debug_axis", "%d", rxConfig()->rc_smoothing_debug_axis); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rxConfig()->rc_smoothing_input_cutoff, - rxConfig()->rc_smoothing_derivative_cutoff); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor", "%d", rxConfig()->rc_smoothing_auto_factor); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_filter_type", "%d, %d", rxConfig()->rc_smoothing_input_type, - rxConfig()->rc_smoothing_derivative_type); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE), - rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE)); - BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME)); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_type", "%d", rcSmoothingData->inputFilterType); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_debug_axis", "%d", rcSmoothingData->debugAxis); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rcSmoothingData->inputCutoffSetting, + rcSmoothingData->derivativeCutoffSetting); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor", "%d", rcSmoothingData->autoSmoothnessFactor); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_filter_type", "%d, %d", rcSmoothingData->inputFilterType, + rcSmoothingData->derivativeFilterType); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingData->inputCutoffFrequency, + rcSmoothingData->derivativeCutoffFrequency); + BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingData->averageFrameTimeUs); #endif // USE_RC_SMOOTHING_FILTER diff --git a/src/main/cli/cli.c b/src/main/cli/cli.c index f0ea259525..fc382c61b8 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -4422,10 +4422,11 @@ static void cliVersion(char *cmdline) static void cliRcSmoothing(char *cmdline) { UNUSED(cmdline); + rcSmoothingFilter_t *rcSmoothingData = getRcSmoothingData(); cliPrint("# RC Smoothing Type: "); if (rxConfig()->rc_smoothing_type == RC_SMOOTHING_TYPE_FILTER) { cliPrintLine("FILTER"); - uint16_t avgRxFrameMs = rcSmoothingGetValue(RC_SMOOTHING_VALUE_AVERAGE_FRAME); + uint16_t avgRxFrameMs = rcSmoothingData->averageFrameTimeUs; if (rcSmoothingAutoCalculate()) { cliPrint("# Detected RX frame rate: "); if (avgRxFrameMs == 0) { @@ -4435,20 +4436,20 @@ static void cliRcSmoothing(char *cmdline) } } cliPrint("# Input filter type: "); - cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rxConfig()->rc_smoothing_input_type]); - cliPrintf("# Active input cutoff: %dhz ", rcSmoothingGetValue(RC_SMOOTHING_VALUE_INPUT_ACTIVE)); - if (rxConfig()->rc_smoothing_input_cutoff == 0) { + cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rcSmoothingData->inputFilterType]); + cliPrintf("# Active input cutoff: %dhz ", rcSmoothingData->inputCutoffFrequency); + if (rcSmoothingData->inputCutoffSetting == 0) { cliPrintLine("(auto)"); } else { cliPrintLine("(manual)"); } cliPrint("# Derivative filter type: "); - cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rxConfig()->rc_smoothing_derivative_type]); - cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingGetValue(RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE)); - if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_OFF) { + cliPrintLinef(lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rcSmoothingData->derivativeFilterType]); + cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingData->derivativeCutoffFrequency); + if (rcSmoothingData->derivativeFilterType == RC_SMOOTHING_DERIVATIVE_OFF) { cliPrintLine("off)"); } else { - if (rxConfig()->rc_smoothing_derivative_cutoff == 0) { + if (rcSmoothingData->derivativeCutoffSetting == 0) { cliPrintLine("auto)"); } else { cliPrintLine("manual)"); diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 8b046d9ca3..d5f4671868 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -286,10 +286,10 @@ FAST_CODE uint8_t processRcInterpolation(void) #ifdef USE_RC_SMOOTHING_FILTER // Determine a cutoff frequency based on filter type and the calculated // average rx frame time -FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1) +FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1, uint8_t autoSmoothnessFactor) { if (avgRxFrameTimeUs > 0) { - const float cutoffFactor = (100 - rxConfig()->rc_smoothing_auto_factor) / 100.0f; + const float cutoffFactor = (100 - autoSmoothnessFactor) / 100.0f; float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)) / 2; // calculate the nyquist frequency cutoff = cutoff * cutoffFactor; @@ -316,15 +316,15 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi const float dT = targetPidLooptime * 1e-6f; uint16_t oldCutoff = smoothingData->inputCutoffFrequency; - if (rxConfig()->rc_smoothing_input_cutoff == 0) { - smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_input_type == RC_SMOOTHING_INPUT_PT1)); + if (smoothingData->inputCutoffSetting == 0) { + smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (smoothingData->inputFilterType == RC_SMOOTHING_INPUT_PT1), smoothingData->autoSmoothnessFactor); } // initialize or update the input filter if ((smoothingData->inputCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) { for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) { if ((1 << i) & interpolationChannels) { // only update channels specified by rc_interp_ch - switch (rxConfig()->rc_smoothing_input_type) { + switch (smoothingData->inputFilterType) { case RC_SMOOTHING_INPUT_PT1: if (!smoothingData->filterInitialized) { @@ -349,12 +349,12 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi // update or initialize the derivative filter oldCutoff = smoothingData->derivativeCutoffFrequency; - if ((rxConfig()->rc_smoothing_derivative_cutoff == 0) && (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF)) { - smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_PT1)); + if ((smoothingData->derivativeCutoffSetting == 0) && (smoothingData->derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) { + smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (smoothingData->derivativeFilterType == RC_SMOOTHING_DERIVATIVE_PT1), smoothingData->autoSmoothnessFactor); } if (!smoothingData->filterInitialized) { - pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, rxConfig()->rc_smoothing_debug_axis, rxConfig()->rc_smoothing_derivative_type); + pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, smoothingData->debugAxis, smoothingData->derivativeFilterType); } else if (smoothingData->derivativeCutoffFrequency != oldCutoff) { pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency); } @@ -395,13 +395,13 @@ FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void) bool ret = false; // if the input cutoff is 0 (auto) then we need to calculate cutoffs - if (rxConfig()->rc_smoothing_input_cutoff == 0) { + if (rcSmoothingData.inputCutoffSetting == 0) { ret = true; } // if the derivative type isn't OFF and the cutoff is 0 then we need to calculate - if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) { - if (rxConfig()->rc_smoothing_derivative_cutoff == 0) { + if (rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF) { + if (rcSmoothingData.derivativeCutoffSetting == 0) { ret = true; } } @@ -421,12 +421,18 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) initialized = true; rcSmoothingData.filterInitialized = false; rcSmoothingData.averageFrameTimeUs = 0; + rcSmoothingData.autoSmoothnessFactor = rxConfig()->rc_smoothing_auto_factor; + rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis; + rcSmoothingData.inputFilterType = rxConfig()->rc_smoothing_input_type; + rcSmoothingData.inputCutoffSetting = rxConfig()->rc_smoothing_input_cutoff; + rcSmoothingData.derivativeFilterType = rxConfig()->rc_smoothing_derivative_type; + rcSmoothingData.derivativeCutoffSetting = rxConfig()->rc_smoothing_derivative_cutoff; rcSmoothingResetAccumulation(&rcSmoothingData); - rcSmoothingData.inputCutoffFrequency = rxConfig()->rc_smoothing_input_cutoff; + rcSmoothingData.inputCutoffFrequency = rcSmoothingData.inputCutoffSetting; - if (rxConfig()->rc_smoothing_derivative_type != RC_SMOOTHING_DERIVATIVE_OFF) { - rcSmoothingData.derivativeCutoffFrequency = rxConfig()->rc_smoothing_derivative_cutoff; + if (rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF) { + rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting; } calculateCutoffs = rcSmoothingAutoCalculate(); @@ -511,7 +517,7 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) if (rcSmoothingData.filterInitialized && (debugMode == DEBUG_RC_SMOOTHING)) { // after training has completed then log the raw rc channel and the calculated // average rx frame rate that was used to calculate the automatic filter cutoffs - DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rxConfig()->rc_smoothing_debug_axis])); + DEBUG_SET(DEBUG_RC_SMOOTHING, 0, lrintf(lastRxData[rcSmoothingData.debugAxis])); DEBUG_SET(DEBUG_RC_SMOOTHING, 3, rcSmoothingData.averageFrameTimeUs); } @@ -519,7 +525,7 @@ FAST_CODE uint8_t processRcSmoothingFilter(void) for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) { if ((1 << updatedChannel) & interpolationChannels) { // only smooth selected channels base on the rc_interp_ch value if (rcSmoothingData.filterInitialized) { - switch (rxConfig()->rc_smoothing_input_type) { + switch (rcSmoothingData.inputFilterType) { case RC_SMOOTHING_INPUT_PT1: rcCommand[updatedChannel] = pt1FilterApply((pt1Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]); break; @@ -749,18 +755,9 @@ bool rcSmoothingIsEnabled(void) } #ifdef USE_RC_SMOOTHING_FILTER -int rcSmoothingGetValue(int whichValue) +rcSmoothingFilter_t *getRcSmoothingData(void) { - switch (whichValue) { - case RC_SMOOTHING_VALUE_INPUT_ACTIVE: - return rcSmoothingData.inputCutoffFrequency; - case RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE: - return rcSmoothingData.derivativeCutoffFrequency; - case RC_SMOOTHING_VALUE_AVERAGE_FRAME: - return rcSmoothingData.averageFrameTimeUs; - default: - return 0; - } + return &rcSmoothingData; } bool rcSmoothingInitializationComplete(void) { diff --git a/src/main/fc/rc.h b/src/main/fc/rc.h index ae9586bf51..e8a7bcc0f2 100644 --- a/src/main/fc/rc.h +++ b/src/main/fc/rc.h @@ -20,6 +20,8 @@ #pragma once +#include "fc/rc_controls.h" + typedef enum { INTERPOLATION_CHANNELS_RP, INTERPOLATION_CHANNELS_RPY, @@ -40,6 +42,6 @@ void resetYawAxis(void); void initRcProcessing(void); bool isMotorsReversed(void); bool rcSmoothingIsEnabled(void); -int rcSmoothingGetValue(int whichValue); +rcSmoothingFilter_t *getRcSmoothingData(void); bool rcSmoothingAutoCalculate(void); bool rcSmoothingInitializationComplete(void); diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 291a48c820..bdaa78b81c 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -77,12 +77,6 @@ typedef enum { RC_SMOOTHING_DERIVATIVE_BIQUAD } rcSmoothingDerivativeFilter_e; -typedef enum { - RC_SMOOTHING_VALUE_INPUT_ACTIVE, - RC_SMOOTHING_VALUE_DERIVATIVE_ACTIVE, - RC_SMOOTHING_VALUE_AVERAGE_FRAME -} rcSmoothingInfoType_e; - #define ROL_LO (1 << (2 * ROLL)) #define ROL_CE (3 << (2 * ROLL)) #define ROL_HI (2 << (2 * ROLL)) @@ -125,10 +119,16 @@ typedef union rcSmoothingFilterTypes_u { typedef struct rcSmoothingFilter_s { bool filterInitialized; rcSmoothingFilterTypes_t filter[4]; + rcSmoothingInputFilter_e inputFilterType; + uint8_t inputCutoffSetting; uint16_t inputCutoffFrequency; + rcSmoothingDerivativeFilter_e derivativeFilterType; + uint8_t derivativeCutoffSetting; uint16_t derivativeCutoffFrequency; int averageFrameTimeUs; rcSmoothingFilterTraining_t training; + uint8_t debugAxis; + uint8_t autoSmoothnessFactor; } rcSmoothingFilter_t; typedef struct rcControlsConfig_s {