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 eae7a8153d..c51da048e5 100644 --- a/src/main/cli/cli.c +++ b/src/main/cli/cli.c @@ -4453,10 +4453,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) { @@ -4466,20 +4467,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/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/rc.c b/src/main/fc/rc.c index abaf19d752..e9e30692b8 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -297,10 +297,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; @@ -327,15 +327,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) { @@ -360,12 +360,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); } @@ -406,13 +406,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; } } @@ -432,12 +432,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(); @@ -522,7 +528,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); } @@ -530,7 +536,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; @@ -764,18 +770,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 { 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 5bd6138edc..3c78fb940c 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); @@ -862,6 +870,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; @@ -2433,11 +2445,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);