From 3cee0c99cc932dcf9c964fa1b263d0acb2a7b999 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Wed, 15 May 2019 08:21:15 -0400 Subject: [PATCH 1/2] Use RC Smoothing initialized settings rather than current PG values Prevents possibility of changing paramaters after initializing affecting runtime operation of RC smoothing. Parameter values are loaded during initialization instead of relying on the current PG values. --- src/main/blackbox/blackbox.c | 24 ++++++++++------- src/main/cli/cli.c | 17 ++++++------ src/main/fc/rc.c | 51 +++++++++++++++++------------------- src/main/fc/rc.h | 4 ++- src/main/fc/rc_controls.h | 12 ++++----- 5 files changed, 56 insertions(+), 52 deletions(-) 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 { From 25435ea49cbff8ef063c4f50c4e4dc85bbe1ac29 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Sun, 19 May 2019 08:58:44 -0400 Subject: [PATCH 2/2] Add reboot required arming disabled flag, support for setting in MSP --- src/main/fc/config.c | 14 ++++++++++++++ src/main/fc/config.h | 3 +++ src/main/fc/runtime_config.c | 3 ++- src/main/fc/runtime_config.h | 3 ++- src/main/msp/msp.c | 22 +++++++++++++++++----- 5 files changed, 38 insertions(+), 7 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index b98692bcc3..6358d89f40 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -41,6 +41,7 @@ #include "fc/rc.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" +#include "fc/runtime_config.h" #include "flight/failsafe.h" #include "flight/imu.h" @@ -72,6 +73,8 @@ static bool configIsDirty; /* someone indicated that the config is modified and it is not yet saved */ +static bool rebootRequired = false; // set if a config change requires a reboot to take effect + pidProfile_t *currentPidProfile; #ifndef RX_SPI_DEFAULT_PROTOCOL @@ -742,3 +745,14 @@ bool isSystemConfigured(void) return true; #endif } + +void setRebootRequired(void) +{ + rebootRequired = true; + setArmingDisabled(ARMING_DISABLED_REBOOT_REQUIRED); +} + +bool getRebootRequired(void) +{ + return rebootRequired; +} diff --git a/src/main/fc/config.h b/src/main/fc/config.h index e692a22ec6..4ca1b9fa98 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -55,6 +55,7 @@ PG_DECLARE(systemConfig_t, systemConfig); struct pidProfile_s; extern struct pidProfile_s *currentPidProfile; + void initEEPROM(void); void resetEEPROM(void); bool readEEPROM(void); @@ -86,3 +87,5 @@ void targetConfiguration(void); void targetValidateConfiguration(void); bool isSystemConfigured(void); +void setRebootRequired(void); +bool getRebootRequired(void); diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index b878d7c935..3ed02e8847 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -53,8 +53,9 @@ const char *armingDisableFlagNames[]= { "MSP", "PARALYZE", "GPS", - "RESCUE SW", + "RESCUE_SW", "RPMFILTER", + "REBOOT_REQD", "ARMSWITCH", }; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 3017afb0cf..365b35dbe8 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -61,7 +61,8 @@ typedef enum { ARMING_DISABLED_GPS = (1 << 18), ARMING_DISABLED_RESC = (1 << 19), ARMING_DISABLED_RPMFILTER = (1 << 20), - ARMING_DISABLED_ARM_SWITCH = (1 << 21), // Needs to be the last element, since it's always activated if one of the others is active when arming + ARMING_DISABLED_REBOOT_REQUIRED = (1 << 21), + ARMING_DISABLED_ARM_SWITCH = (1 << 22), // Needs to be the last element, since it's always activated if one of the others is active when arming } armingDisableFlags_e; #define ARMING_DISABLE_FLAGS_COUNT (LOG2(ARMING_DISABLED_ARM_SWITCH) + 1) diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 25e7db5ac4..c0b5c349f2 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -241,6 +241,14 @@ static void mspFc4waySerialCommand(sbuf_t *dst, sbuf_t *src, mspPostProcessFnPtr } #endif //USE_SERIAL_4WAY_BLHELI_INTERFACE +static void configRebootUpdateCheckU8(uint8_t *parm, uint8_t value) +{ + if (*parm != value) { + setRebootRequired(); + } + *parm = value; +} + static void mspRebootFn(serialPort_t *serialPort) { UNUSED(serialPort); @@ -860,6 +868,10 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) // 4 bytes, flags const uint32_t armingDisableFlags = getArmingDisableFlags(); sbufWriteU32(dst, armingDisableFlags); + + // config state flags - bits to indicate the state of the configuration, reboot required, etc. + // other flags can be added as needed + sbufWriteU8(dst, (getRebootRequired() << 0)); } break; @@ -2431,11 +2443,11 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) // Added in MSP API 1.40 rxConfigMutable()->rcInterpolationChannels = sbufReadU8(src); #if defined(USE_RC_SMOOTHING_FILTER) - rxConfigMutable()->rc_smoothing_type = sbufReadU8(src); - rxConfigMutable()->rc_smoothing_input_cutoff = sbufReadU8(src); - rxConfigMutable()->rc_smoothing_derivative_cutoff = sbufReadU8(src); - rxConfigMutable()->rc_smoothing_input_type = sbufReadU8(src); - rxConfigMutable()->rc_smoothing_derivative_type = sbufReadU8(src); + configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_type, sbufReadU8(src)); + configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_input_cutoff, sbufReadU8(src)); + configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_derivative_cutoff, sbufReadU8(src)); + configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_input_type, sbufReadU8(src)); + configRebootUpdateCheckU8(&rxConfigMutable()->rc_smoothing_derivative_type, sbufReadU8(src)); #else sbufReadU8(src); sbufReadU8(src);