From 06fed7bec2315e80e6362dffba30dcdf22cb6d66 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 27 Apr 2021 16:44:39 +0200 Subject: [PATCH] First cut on Smith Compensator on PID measurement --- src/main/CMakeLists.txt | 2 + src/main/build/debug.h | 1 + src/main/fc/settings.yaml | 24 ++++++++++- src/main/flight/pid.c | 34 +++++++++++++++ src/main/flight/pid.h | 4 ++ src/main/flight/smith_predictor.c | 69 +++++++++++++++++++++++++++++++ src/main/flight/smith_predictor.h | 45 ++++++++++++++++++++ src/main/target/common.h | 1 + 8 files changed, 179 insertions(+), 1 deletion(-) create mode 100644 src/main/flight/smith_predictor.c create mode 100644 src/main/flight/smith_predictor.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index a827532ef2..28453ab2df 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -320,6 +320,8 @@ main_sources(COMMON_SRC flight/imu.h flight/kalman.c flight/kalman.h + flight/smith_predictor.c + flight/smith_predictor.h flight/mixer.c flight/mixer.h flight/pid.c diff --git a/src/main/build/debug.h b/src/main/build/debug.h index e8f47543eb..9f0be806b2 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -83,5 +83,6 @@ typedef enum { DEBUG_FW_D, DEBUG_IMU2, DEBUG_ALTITUDE, + DEBUG_SMITH_COMPENSATOR, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c40ca0d1cb..1ea674a303 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -90,7 +90,8 @@ tables: "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", - "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE"] + "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE", + "SMITH_COMPENSATOR"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator @@ -2058,6 +2059,27 @@ groups: field: fixedWingLevelTrim min: -10 max: 10 + - name: smith_predictor_strength + description: "The strength factor of a Smith Predictor of PID measurement. In percents" + default_value: 50 + field: smithPredictorStrength + condition: USE_SMITH_PREDICTOR + min: 0 + max: 100 + - name: smith_predictor_delay + description: "Expected delay of the gyro signal. In tenths of a millisecond. 32 means 3.2ms" + default_value: 32 + field: smithPredictorDelay + condition: USE_SMITH_PREDICTOR + min: 0 + max: 80 + - name: smith_predictor_lpf_hz + description: "Cutoff frequency for the Smith Predictor Low Pass Filter" + default_value: 50 + field: smithPredictorFilterHz + condition: USE_SMITH_PREDICTOR + min: 1 + max: 500 - name: PG_PID_AUTOTUNE_CONFIG type: pidAutotuneConfig_t diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b8c4b94e24..b66e2c3685 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -48,6 +48,7 @@ FILE_COMPILE_FOR_SPEED #include "flight/rpm_filter.h" #include "flight/secondary_imu.h" #include "flight/kalman.h" +#include "flight/smith_predictor.h" #include "io/gps.h" @@ -109,6 +110,8 @@ typedef struct { bool itermFreezeActive; biquadFilter_t rateTargetFilter; + + smithPredictor_t smithPredictor; } pidState_t; STATIC_FASTRAM bool pidFiltersConfigured = false; @@ -349,6 +352,30 @@ bool pidInitFilters(void) } } +#ifdef USE_SMITH_PREDICTOR + smithPredictorInit( + &pidState[FD_ROLL].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, + getLooptime() + ); + smithPredictorInit( + &pidState[FD_PITCH].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, + getLooptime() + ); + smithPredictorInit( + &pidState[FD_YAW].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, + getLooptime() + ); +#endif + pidFiltersConfigured = true; return true; @@ -1052,6 +1079,13 @@ void FAST_CODE pidController(float dT) pidState[axis].gyroRate = gyroKalmanUpdate(axis, pidState[axis].gyroRate, pidState[axis].rateTarget); } #endif + +#ifdef USE_SMITH_PREDICTOR + DEBUG_SET(DEBUG_SMITH_COMPENSATOR, axis, pidState[axis].gyroRate); + pidState[axis].gyroRate = applySmithPredictor(axis, &pidState[axis].smithPredictor, pidState[axis].gyroRate); + DEBUG_SET(DEBUG_SMITH_COMPENSATOR, axis + 3, pidState[axis].gyroRate); +#endif + DEBUG_SET(DEBUG_PID_MEASUREMENT, axis, pidState[axis].gyroRate); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3f3d5a3bcc..824089e16e 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -161,6 +161,10 @@ typedef struct pidProfile_s { #endif float fixedWingLevelTrim; + + uint8_t smithPredictorStrength; + uint8_t smithPredictorDelay; + uint16_t smithPredictorFilterHz; } pidProfile_t; typedef struct pidAutotuneConfig_s { diff --git a/src/main/flight/smith_predictor.c b/src/main/flight/smith_predictor.c new file mode 100644 index 0000000000..849364b63f --- /dev/null +++ b/src/main/flight/smith_predictor.c @@ -0,0 +1,69 @@ +/* + * 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/. + * + * This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight + * + */ + +#include "platform.h" +#ifdef USE_SMITH_PREDICTOR + +#include +#include "common/axis.h" +#include "flight/smith_predictor.h" +#include "build/debug.h" + +FUNCTION_COMPILE_FOR_SPEED +float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample) { + UNUSED(axis); + if (predictor->enabled) { + predictor->data[predictor->idx] = sample; + + predictor->idx++; + if (predictor->idx > predictor->samples) { + predictor->idx = 0; + } + + // filter the delayedGyro to help reduce the overall noise this prediction adds + float delayed = pt1FilterApply(&predictor->smithPredictorFilter, predictor->data[predictor->idx]); + float delayCompensatedSample = predictor->smithPredictorStrength * (sample - delayed); + + sample += delayCompensatedSample; + } + return sample; +} + +FUNCTION_COMPILE_FOR_SIZE +void smithPredictorInit(smithPredictor_t *predictor, uint8_t delay, uint8_t strength, uint16_t filterLpfHz, uint32_t looptime) { + if (delay > 0) { + predictor->enabled = true; + predictor->samples = delay / (looptime / 100.0f); + predictor->idx = 0; + predictor->smithPredictorStrength = strength / 100.0f; + pt1FilterInit(&predictor->smithPredictorFilter, filterLpfHz, looptime * 1e-6f); + } else { + predictor->enabled = false; + } +} + +#endif \ No newline at end of file diff --git a/src/main/flight/smith_predictor.h b/src/main/flight/smith_predictor.h new file mode 100644 index 0000000000..cecbea8c4d --- /dev/null +++ b/src/main/flight/smith_predictor.h @@ -0,0 +1,45 @@ +/* + * 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/. + * + * This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight + * + */ + +#pragma once + +#include +#include "common/filter.h" + +#define MAX_SMITH_SAMPLES 64 + +typedef struct smithPredictor_s { + bool enabled; + uint8_t samples; + uint8_t idx; + float data[MAX_SMITH_SAMPLES + 1]; + pt1Filter_t smithPredictorFilter; + float smithPredictorStrength; +} smithPredictor_t; + +float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample); +void smithPredictorInit(smithPredictor_t *predictor, uint8_t delay, uint8_t strength, uint16_t filterLpfHz, uint32_t looptime); \ No newline at end of file diff --git a/src/main/target/common.h b/src/main/target/common.h index c0310e7da8..41fc44229f 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -86,6 +86,7 @@ #define USE_DYNAMIC_FILTERS #define USE_GYRO_KALMAN +#define USE_SMITH_PREDICTOR #define USE_EXTENDED_CMS_MENUS #define USE_HOTT_TEXTMODE