diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 6e7d914b25a..3a8d8d1c8ca 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -3,24 +3,19 @@ { "name": "Linux", "includePath": [ - "${workspaceRoot}/src/main/**", - "${workspaceRoot}/lib/main/**", - "/usr/include/**" + "${workspaceRoot}", + "${workspaceRoot}/src/main/**" ], "browse": { "limitSymbolsToIncludedHeaders": false, "path": [ - "${workspaceRoot}/src/main/**", - "${workspaceRoot}/lib/main/**" + "${workspaceRoot}/**" ] }, - "intelliSenseMode": "linux-gcc-arm", + "intelliSenseMode": "msvc-x64", "cStandard": "c11", "cppStandard": "c++17", "defines": [ - "MCU_FLASH_SIZE 512", - "USE_NAV", - "NAV_FIXED_WING_LANDING", "USE_OSD", "USE_GYRO_NOTCH_1", "USE_GYRO_NOTCH_2", @@ -34,30 +29,13 @@ "USE_RPM_FILTER", "USE_GLOBAL_FUNCTIONS", "USE_DYNAMIC_FILTERS", - "USE_IMU_BNO055", - "USE_SECONDARY_IMU", "USE_DSHOT", "FLASH_SIZE 480", "USE_I2C_IO_EXPANDER", "USE_PCF8574", "USE_ESC_SENSOR", - "USE_PROGRAMMING_FRAMEWORK", - "USE_SERIALRX_GHST", - "USE_TELEMETRY_GHST", - "USE_CMS", - "USE_DJI_HD_OSD", - "USE_GYRO_KALMAN", - "USE_RANGEFINDER", - "USE_RATE_DYNAMICS", - "USE_SMITH_PREDICTOR", - "USE_ALPHA_BETA_GAMMA_FILTER", - "USE_MAG_VCM5883", - "USE_TELEMETRY_JETIEXBUS", - "USE_NAV", - "USE_SDCARD_SDIO", - "USE_SDCARD", - "USE_Q_TUNE", - "USE_GYRO_FFT_FILTER" + "USE_ADAPTIVE_FILTER", + "MCU_FLASH_SIZE 1024", ], "configurationProvider": "ms-vscode.cmake-tools" } diff --git a/cmake/at32.cmake b/cmake/at32.cmake index 6ac70bc528e..bd6f1e28196 100644 --- a/cmake/at32.cmake +++ b/cmake/at32.cmake @@ -18,6 +18,7 @@ set(CMSIS_DSP_SRC BasicMathFunctions/arm_scale_f32.c BasicMathFunctions/arm_sub_f32.c BasicMathFunctions/arm_mult_f32.c + BasicMathFunctions/arm_offset_f32.c TransformFunctions/arm_rfft_fast_f32.c TransformFunctions/arm_cfft_f32.c TransformFunctions/arm_rfft_fast_init_f32.c @@ -26,6 +27,9 @@ set(CMSIS_DSP_SRC CommonTables/arm_common_tables.c ComplexMathFunctions/arm_cmplx_mag_f32.c StatisticsFunctions/arm_max_f32.c + StatisticsFunctions/arm_rms_f32.c + StatisticsFunctions/arm_std_f32.c + StatisticsFunctions/arm_mean_f32.c ) list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/") diff --git a/cmake/stm32.cmake b/cmake/stm32.cmake index c4534a30500..091cf31fd5d 100644 --- a/cmake/stm32.cmake +++ b/cmake/stm32.cmake @@ -19,6 +19,7 @@ set(CMSIS_DSP_SRC BasicMathFunctions/arm_scale_f32.c BasicMathFunctions/arm_sub_f32.c BasicMathFunctions/arm_mult_f32.c + BasicMathFunctions/arm_offset_f32.c TransformFunctions/arm_rfft_fast_f32.c TransformFunctions/arm_cfft_f32.c TransformFunctions/arm_rfft_fast_init_f32.c @@ -27,6 +28,9 @@ set(CMSIS_DSP_SRC CommonTables/arm_common_tables.c ComplexMathFunctions/arm_cmplx_mag_f32.c StatisticsFunctions/arm_max_f32.c + StatisticsFunctions/arm_rms_f32.c + StatisticsFunctions/arm_std_f32.c + StatisticsFunctions/arm_mean_f32.c ) list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/") diff --git a/docs/Settings.md b/docs/Settings.md index 3d0758ab2a2..63a13285982 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1582,6 +1582,76 @@ For developer ground test use. Disables motors, sets heading status = Trusted on --- +### gyro_adaptive_filter_hpf_hz + +High pass filter cutoff frequency + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 1 | 50 | + +--- + +### gyro_adaptive_filter_integrator_threshold_high + +High threshold for adaptive filter integrator + +| Default | Min | Max | +| --- | --- | --- | +| 4 | 1 | 10 | + +--- + +### gyro_adaptive_filter_integrator_threshold_low + +Low threshold for adaptive filter integrator + +| Default | Min | Max | +| --- | --- | --- | +| -2 | -10 | 0 | + +--- + +### gyro_adaptive_filter_max_hz + +Maximum frequency for adaptive filter + +| Default | Min | Max | +| --- | --- | --- | +| 150 | 100 | 500 | + +--- + +### gyro_adaptive_filter_min_hz + +Minimum frequency for adaptive filter + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 30 | 250 | + +--- + +### gyro_adaptive_filter_std_lpf_hz + +Standard deviation low pass filter cutoff frequency + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 10 | + +--- + +### gyro_adaptive_filter_target + +Target value for adaptive filter + +| Default | Min | Max | +| --- | --- | --- | +| 3.5 | 1 | 6 | + +--- + ### gyro_anti_aliasing_lpf_hz Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz @@ -1622,33 +1692,33 @@ Minimum frequency of the gyro Dynamic LPF --- -### gyro_main_lpf_hz +### gyro_filter_mode -Software based gyro main lowpass filter. Value is cutoff frequency (Hz) +Specifies the type of the software LPF of the gyro signals. | Default | Min | Max | | --- | --- | --- | -| 60 | 0 | 500 | +| STATIC | | | --- -### gyro_to_use +### gyro_main_lpf_hz -On multi-gyro targets, allows to choose which gyro to use. 0 = first gyro, 1 = second gyro +Software based gyro main lowpass filter. Value is cutoff frequency (Hz) | Default | Min | Max | | --- | --- | --- | -| 0 | 0 | 2 | +| 60 | 0 | 500 | --- -### gyro_use_dyn_lpf +### gyro_to_use -Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. +On multi-gyro targets, allows to choose which gyro to use. 0 = first gyro, 1 = second gyro | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| 0 | 0 | 2 | --- diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index c87abc1782d..5e5efd62ec8 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -346,6 +346,8 @@ main_sources(COMMON_SRC flight/dynamic_lpf.h flight/ez_tune.c flight/ez_tune.h + flight/adaptive_filter.c + flight/adaptive_filter.h io/adsb.c io/beeper.c diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 3a09da50daf..60b22df27bc 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -72,5 +72,6 @@ typedef enum { DEBUG_RATE_DYNAMICS, DEBUG_LANDING, DEBUG_POS_EST, + DEBUG_ADAPTIVE_FILTER, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index e5f164988df..dc249df059b 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -190,6 +190,18 @@ uint32_t getGyroLooptime(void) void validateAndFixConfig(void) { + +#ifdef USE_ADAPTIVE_FILTER + // gyroConfig()->adaptiveFilterMinHz has to be at least 5 units lower than gyroConfig()->gyro_main_lpf_hz + if (gyroConfig()->adaptiveFilterMinHz + 5 > gyroConfig()->gyro_main_lpf_hz) { + gyroConfigMutable()->adaptiveFilterMinHz = gyroConfig()->gyro_main_lpf_hz - 5; + } + //gyroConfig()->adaptiveFilterMaxHz has to be at least 5 units higher than gyroConfig()->gyro_main_lpf_hz + if (gyroConfig()->adaptiveFilterMaxHz - 5 < gyroConfig()->gyro_main_lpf_hz) { + gyroConfigMutable()->adaptiveFilterMaxHz = gyroConfig()->gyro_main_lpf_hz + 5; + } +#endif + if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) { accelerometerConfigMutable()->acc_notch_hz = 0; } diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 406622af38f..0fd239127c2 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -51,6 +51,7 @@ #include "flight/rpm_filter.h" #include "flight/servos.h" #include "flight/wind_estimator.h" +#include "flight/adaptive_filter.h" #include "navigation/navigation.h" @@ -426,6 +427,10 @@ void fcTasksInit(void) setTaskEnabled(TASK_SMARTPORT_MASTER, true); #endif +#ifdef USE_ADAPTIVE_FILTER + setTaskEnabled(TASK_ADAPTIVE_FILTER, (gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE)); +#endif + #if defined(SITL_BUILD) serialProxyStart(); #endif @@ -680,4 +685,12 @@ cfTask_t cfTasks[TASK_COUNT] = { .desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ), // 100Hz @10ms .staticPriority = TASK_PRIORITY_HIGH, }, +#ifdef USE_ADAPTIVE_FILTER + [TASK_ADAPTIVE_FILTER] = { + .taskName = "ADAPTIVE_FILTER", + .taskFunc = adaptiveFilterTask, + .desiredPeriod = TASK_PERIOD_HZ(ADAPTIVE_FILTER_RATE_HZ), // 100Hz @10ms + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4a22ecb8711..c224aef0c03 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -83,7 +83,7 @@ tables: values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", - "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST"] + "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER"] - name: aux_operator values: ["OR", "AND"] enum: modeActivationOperator_e @@ -191,6 +191,9 @@ tables: - name: led_pin_pwm_mode values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"] enum: led_pin_pwm_mode_e + - name: gyro_filter_mode + values: ["STATIC", "DYNAMIC", "ADAPTIVE"] + enum: gyroFilterType_e constants: RPYL_PID_MIN: 0 @@ -226,11 +229,11 @@ groups: field: gyro_main_lpf_hz min: 0 max: 500 - - name: gyro_use_dyn_lpf - description: "Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position." - default_value: OFF - field: useDynamicLpf - type: bool + - name: gyro_filter_mode + description: "Specifies the type of the software LPF of the gyro signals." + default_value: "STATIC" + field: gyroFilterMode + table: gyro_filter_mode - name: gyro_dyn_lpf_min_hz description: "Minimum frequency of the gyro Dynamic LPF" default_value: 200 @@ -330,6 +333,55 @@ groups: field: gravity_cmss_cal min: 0 max: 2000 + - name: gyro_adaptive_filter_target + description: "Target value for adaptive filter" + default_value: 3.5 + field: adaptiveFilterTarget + min: 1 + max: 6 + condition: USE_ADAPTIVE_FILTER + - name: gyro_adaptive_filter_min_hz + description: "Minimum frequency for adaptive filter" + default_value: 50 + field: adaptiveFilterMinHz + min: 30 + max: 250 + condition: USE_ADAPTIVE_FILTER + - name: gyro_adaptive_filter_max_hz + description: "Maximum frequency for adaptive filter" + default_value: 150 + field: adaptiveFilterMaxHz + min: 100 + max: 500 + condition: USE_ADAPTIVE_FILTER + - name: gyro_adaptive_filter_std_lpf_hz + description: "Standard deviation low pass filter cutoff frequency" + default_value: 2 + field: adaptiveFilterStdLpfHz + min: 0 + max: 10 + condition: USE_ADAPTIVE_FILTER + - name: gyro_adaptive_filter_hpf_hz + description: "High pass filter cutoff frequency" + default_value: 10 + field: adaptiveFilterHpfHz + min: 1 + max: 50 + condition: USE_ADAPTIVE_FILTER + - name: gyro_adaptive_filter_integrator_threshold_high + description: "High threshold for adaptive filter integrator" + default_value: 4 + field: adaptiveFilterIntegratorThresholdHigh + min: 1 + max: 10 + condition: USE_ADAPTIVE_FILTER + - name: gyro_adaptive_filter_integrator_threshold_low + description: "Low threshold for adaptive filter integrator" + default_value: -2 + field: adaptiveFilterIntegratorThresholdLow + min: -10 + max: 0 + condition: USE_ADAPTIVE_FILTER - name: PG_ADC_CHANNEL_CONFIG type: adcChannelConfig_t diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c new file mode 100644 index 00000000000..4770e9763ab --- /dev/null +++ b/src/main/flight/adaptive_filter.c @@ -0,0 +1,190 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include "platform.h" + +#ifdef USE_ADAPTIVE_FILTER + +#include +#include "flight/adaptive_filter.h" +#include "arm_math.h" +#include +#include "common/maths.h" +#include "common/axis.h" +#include "common/filter.h" +#include "build/debug.h" +#include "fc/config.h" +#include "fc/runtime_config.h" +#include "fc/rc_controls.h" +#include "sensors/gyro.h" + +STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE]; +STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0; + +STATIC_FASTRAM pt1Filter_t stdFilter[XYZ_AXIS_COUNT]; +STATIC_FASTRAM pt1Filter_t hpfFilter[XYZ_AXIS_COUNT]; + +/* + We want to run adaptive filter only when UAV is commanded to stay stationary + Any rotation request on axis will add noise that we are not interested in as it will + automatically cause LPF frequency to be lowered +*/ +STATIC_FASTRAM float axisAttenuationFactor[XYZ_AXIS_COUNT]; + +STATIC_FASTRAM uint8_t adaptiveFilterInitialized = 0; +STATIC_FASTRAM uint8_t hpfFilterInitialized = 0; + +//Defines if current, min and max values for the filter were set and filter is ready to work +STATIC_FASTRAM uint8_t targetsSet = 0; +STATIC_FASTRAM float currentLpf; +STATIC_FASTRAM float initialLpf; +STATIC_FASTRAM float minLpf; +STATIC_FASTRAM float maxLpf; + +STATIC_FASTRAM float adaptiveFilterIntegrator; +STATIC_FASTRAM float adaptiveIntegratorTarget; + +/** + * This function is called at pid rate, so has to be initialized at PID loop frequency +*/ +void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) { + + if (!hpfFilterInitialized) { + //Initialize the filter + for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + pt1FilterInit(&hpfFilter[axis], gyroConfig()->adaptiveFilterHpfHz, US2S(getLooptime())); + } + hpfFilterInitialized = 1; + } + + //Apply high pass filter, we are not interested in slowly changing values, only noise + const float filteredGyro = value - pt1FilterApply(&hpfFilter[index], value); + + //Push new sample to the buffer so later we can compute RMS and other measures + adaptiveFilterSamples[index][adaptiveFilterSampleIndex] = filteredGyro; + adaptiveFilterSampleIndex = (adaptiveFilterSampleIndex + 1) % ADAPTIVE_FILTER_BUFFER_SIZE; +} + +void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate) { + const float maxRate = configRate * 10.0f; + axisAttenuationFactor[index] = scaleRangef(fabsf(rate), 0.0f, maxRate, 1.0f, 0.0f); + axisAttenuationFactor[index] = constrainf(axisAttenuationFactor[index], 0.0f, 1.0f); +} + +void adaptiveFilterResetIntegrator(void) { + adaptiveFilterIntegrator = 0.0f; +} + +void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max) { + currentLpf = lpf; + minLpf = min; + maxLpf = max; + initialLpf = currentLpf; + + targetsSet = 1; +} + +void adaptiveFilterTask(timeUs_t currentTimeUs) { + + //If we don't have current, min and max values for the filter, we can't run it yet + if (!targetsSet) { + return; + } + + static timeUs_t previousUpdateTimeUs = 0; + + //Initialization procedure, filter setup etc. + if (!adaptiveFilterInitialized) { + adaptiveIntegratorTarget = 3.5f; + previousUpdateTimeUs = currentTimeUs; + + //Initialize the filter + for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + pt1FilterInit(&stdFilter[axis], gyroConfig()->adaptiveFilterStdLpfHz, 1.0f / ADAPTIVE_FILTER_RATE_HZ); + } + adaptiveFilterInitialized = 1; + } + + //If not armed, leave this routine but reset integrator and set default LPF + if (!ARMING_FLAG(ARMED)) { + currentLpf = initialLpf; + adaptiveFilterResetIntegrator(); + gyroUpdateDynamicLpf(currentLpf); + return; + } + + //Do not run adaptive filter when throttle is low + if (rcCommand[THROTTLE] < 1200) { + return; + } + + //Prepare time delta to normalize time factor of the integrator + const float dT = US2S(currentTimeUs - previousUpdateTimeUs); + previousUpdateTimeUs = currentTimeUs; + + float combinedStd = 0.0f; + + //Compute RMS for each axis + for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + + //Copy axis samples to a temporary buffer + float32_t tempBuffer[ADAPTIVE_FILTER_BUFFER_SIZE]; + + //Copute STD from buffer using arm_std_f32 + float32_t std; + memcpy(tempBuffer, adaptiveFilterSamples[axis], sizeof(adaptiveFilterSamples[axis])); + arm_std_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &std); + + const float filteredStd = pt1FilterApply(&stdFilter[axis], std); + const float error = filteredStd - adaptiveIntegratorTarget; + const float adjustedError = error * axisAttenuationFactor[axis]; + const float timeAdjustedError = adjustedError * dT; + + //Put into integrator + adaptiveFilterIntegrator += timeAdjustedError; + + combinedStd += std; + } + + if (adaptiveFilterIntegrator > gyroConfig()->adaptiveFilterIntegratorThresholdHigh) { + //In this case there is too much noise, we need to lower the LPF frequency + currentLpf = constrainf(currentLpf - 1.0f, minLpf, maxLpf); + gyroUpdateDynamicLpf(currentLpf); + adaptiveFilterResetIntegrator(); + } else if (adaptiveFilterIntegrator < gyroConfig()->adaptiveFilterIntegratorThresholdLow) { + //In this case there is too little noise, we can to increase the LPF frequency + currentLpf = constrainf(currentLpf + 1.0f, minLpf, maxLpf); + gyroUpdateDynamicLpf(currentLpf); + adaptiveFilterResetIntegrator(); + } + + combinedStd /= XYZ_AXIS_COUNT; + + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedStd * 1000.0f); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, adaptiveFilterIntegrator * 10.0f); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, currentLpf); +} + + +#endif /* USE_ADAPTIVE_FILTER */ \ No newline at end of file diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h new file mode 100644 index 00000000000..c8696a91f8a --- /dev/null +++ b/src/main/flight/adaptive_filter.h @@ -0,0 +1,35 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include "common/axis.h" +#include "common/time.h" + +#define ADAPTIVE_FILTER_BUFFER_SIZE 64 +#define ADAPTIVE_FILTER_RATE_HZ 100 + +void adaptiveFilterPush(const flight_dynamics_index_t index, const float value); +void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate); +void adaptiveFilterResetIntegrator(void); +void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max); +void adaptiveFilterTask(timeUs_t currentTimeUs); diff --git a/src/main/flight/dynamic_lpf.c b/src/main/flight/dynamic_lpf.c index 0e5b4f6ef28..2c10613ede7 100644 --- a/src/main/flight/dynamic_lpf.c +++ b/src/main/flight/dynamic_lpf.c @@ -37,7 +37,7 @@ static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLp void dynamicLpfGyroTask(void) { - if (!gyroConfig()->useDynamicLpf) { + if (gyroConfig()->gyroFilterMode != GYRO_FILTER_MODE_DYNAMIC) { return; } diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index 4bde5b645e8..e67e3fe9b47 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -109,7 +109,7 @@ void ezTuneUpdate(void) { #endif //Disable dynamic LPF - gyroConfigMutable()->useDynamicLpf = 0; + gyroConfigMutable()->gyroFilterMode = GYRO_FILTER_MODE_STATIC; //Setup PID controller diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index cb6502b0107..98aa47ebbfa 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -47,6 +47,7 @@ #include "flight/rpm_filter.h" #include "flight/kalman.h" #include "flight/smith_predictor.h" +#include "flight/adaptive_filter.h" #include "io/gps.h" @@ -1205,6 +1206,10 @@ void FAST_CODE pidController(float dT) // Limit desired rate to something gyro can measure reliably pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); + +#ifdef USE_ADAPTIVE_FILTER + adaptiveFilterPushRate(axis, pidState[axis].rateTarget, currentControlRateProfile->stabilized.rates[axis]); +#endif #ifdef USE_GYRO_KALMAN gyroKalmanUpdateSetpoint(axis, pidState[axis].rateTarget); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 0d91876cb72..c1e0ab6bb53 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -121,6 +121,9 @@ typedef enum { #endif #ifdef USE_IRLOCK TASK_IRLOCK, +#endif +#ifdef USE_ADAPTIVE_FILTER + TASK_ADAPTIVE_FILTER, #endif /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 97e3f3be436..2b0b7d8c9bd 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -68,6 +68,7 @@ #include "flight/gyroanalyse.h" #include "flight/rpm_filter.h" #include "flight/kalman.h" +#include "flight/adaptive_filter.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -95,7 +96,7 @@ EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 8); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 10); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT, @@ -104,7 +105,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT, #endif .gyro_main_lpf_hz = SETTING_GYRO_MAIN_LPF_HZ_DEFAULT, - .useDynamicLpf = SETTING_GYRO_USE_DYN_LPF_DEFAULT, .gyroDynamicLpfMinHz = SETTING_GYRO_DYN_LPF_MIN_HZ_DEFAULT, .gyroDynamicLpfMaxHz = SETTING_GYRO_DYN_LPF_MAX_HZ_DEFAULT, .gyroDynamicLpfCurveExpo = SETTING_GYRO_DYN_LPF_CURVE_EXPO_DEFAULT, @@ -122,6 +122,16 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .init_gyro_cal_enabled = SETTING_INIT_GYRO_CAL_DEFAULT, .gyro_zero_cal = {SETTING_GYRO_ZERO_X_DEFAULT, SETTING_GYRO_ZERO_Y_DEFAULT, SETTING_GYRO_ZERO_Z_DEFAULT}, .gravity_cmss_cal = SETTING_INS_GRAVITY_CMSS_DEFAULT, +#ifdef USE_ADAPTIVE_FILTER + .adaptiveFilterTarget = SETTING_GYRO_ADAPTIVE_FILTER_TARGET_DEFAULT, + .adaptiveFilterMinHz = SETTING_GYRO_ADAPTIVE_FILTER_MIN_HZ_DEFAULT, + .adaptiveFilterMaxHz = SETTING_GYRO_ADAPTIVE_FILTER_MAX_HZ_DEFAULT, + .adaptiveFilterStdLpfHz = SETTING_GYRO_ADAPTIVE_FILTER_STD_LPF_HZ_DEFAULT, + .adaptiveFilterHpfHz = SETTING_GYRO_ADAPTIVE_FILTER_HPF_HZ_DEFAULT, + .adaptiveFilterIntegratorThresholdHigh = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH_DEFAULT, + .adaptiveFilterIntegratorThresholdLow = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW_DEFAULT, +#endif + .gyroFilterMode = SETTING_GYRO_FILTER_MODE_DEFAULT, ); STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) @@ -249,6 +259,12 @@ static void gyroInitFilters(void) //Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_hz, getLooptime()); +#ifdef USE_ADAPTIVE_FILTER + if (gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE) { + adaptiveFilterSetDefaultFrequency(gyroConfig()->gyro_main_lpf_hz, gyroConfig()->adaptiveFilterMinHz, gyroConfig()->adaptiveFilterMaxHz); + } +#endif + #ifdef USE_GYRO_KALMAN if (gyroConfig()->kalmanEnabled) { gyroKalmanInitialize(gyroConfig()->kalman_q); @@ -439,6 +455,10 @@ void FAST_CODE NOINLINE gyroFilter(void) gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf); +#ifdef USE_ADAPTIVE_FILTER + adaptiveFilterPush(axis, gyroADCf); +#endif + #ifdef USE_DYNAMIC_FILTERS if (dynamicGyroNotchState.enabled) { gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 2ea70c589da..910dac2ea00 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -52,6 +52,12 @@ typedef enum { DYNAMIC_NOTCH_MODE_3D } dynamicGyroNotchMode_e; +typedef enum { + GYRO_FILTER_MODE_STATIC = 0, + GYRO_FILTER_MODE_DYNAMIC = 1, + GYRO_FILTER_MODE_ADAPTIVE = 2 +} gyroFilterMode_e; + typedef struct gyro_s { bool initialized; uint32_t targetLooptime; @@ -69,7 +75,6 @@ typedef struct gyroConfig_s { uint8_t gyro_to_use; #endif uint16_t gyro_main_lpf_hz; - uint8_t useDynamicLpf; uint16_t gyroDynamicLpfMinHz; uint16_t gyroDynamicLpfMaxHz; uint8_t gyroDynamicLpfCurveExpo; @@ -87,6 +92,16 @@ typedef struct gyroConfig_s { bool init_gyro_cal_enabled; int16_t gyro_zero_cal[XYZ_AXIS_COUNT]; float gravity_cmss_cal; +#ifdef USE_ADAPTIVE_FILTER + float adaptiveFilterTarget; + uint16_t adaptiveFilterMinHz; + uint16_t adaptiveFilterMaxHz; + float adaptiveFilterStdLpfHz; + float adaptiveFilterHpfHz; + float adaptiveFilterIntegratorThresholdHigh; + float adaptiveFilterIntegratorThresholdLow; +#endif + uint8_t gyroFilterMode; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 3fbe45a5cab..048626ad8dd 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -97,6 +97,7 @@ #undef USE_BRUSHED_ESC_AUTODETECT #undef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #undef USE_SERIAL_4WAY_SK_BOOTLOADER +#undef USE_ADAPTIVE_FILTER #undef USE_I2C #undef USE_SPI diff --git a/src/main/target/common.h b/src/main/target/common.h index 0b878067c45..408609485ab 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -211,3 +211,4 @@ #endif #define USE_EZ_TUNE +#define USE_ADAPTIVE_FILTER