diff --git a/make/source.mk b/make/source.mk index e12e19aa7f..7d165ffa99 100644 --- a/make/source.mk +++ b/make/source.mk @@ -248,6 +248,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ $(CMSIS_SRC) \ $(DEVICE_STDPERIPH_SRC) \ common/kalman.c \ + common/lulu.c \ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ bus_bst_stm32f30x.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 436871abb4..bfea3e7092 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1218,6 +1218,9 @@ static bool blackboxWriteSysinfo(void) { BLACKBOX_PRINT_HEADER_LINE("mixer_impl", "%d", currentPidProfile->mixer_impl); BLACKBOX_PRINT_HEADER_LINE("mixer_laziness", "%d", currentPidProfile->mixer_laziness); BLACKBOX_PRINT_HEADER_LINE("mixer_yaw_throttle_comp", "%d", currentPidProfile->mixer_yaw_throttle_comp); +#ifdef USE_LULU + BLACKBOX_PRINT_HEADER_LINE("lulu_n_val", "%d", currentPidProfile->lulu_n_val); +#endif // End of EmuFlight controller parameters BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband); BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband); diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 84d93a28a6..10e8dfff88 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -77,5 +77,6 @@ const char * const debugModeNames[DEBUG_COUNT] = { "IMU", "KALMAN", "ANGLE", - "HORIZON" + "HORIZON", + "LULU" }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 8c4a280c11..b9db72f51c 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -96,6 +96,7 @@ typedef enum { DEBUG_KALMAN, DEBUG_ANGLE, DEBUG_HORIZON, + DEBUG_LULU, DEBUG_COUNT } debugType_e; diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 50c0bfc589..551d8ef468 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -87,6 +87,9 @@ static uint8_t tmpRateProfileIndex; static uint8_t rateProfileIndex; static char rateProfileIndexString[] = " p-r"; static controlRateConfig_t rateProfile; +#ifdef USE_LULU +static uint8_t lulu_n_val; +#endif static const char * const cms_offOnLabels[] = { "OFF", "ON" @@ -101,7 +104,10 @@ static const char * const cms_mixerImplTypeLabels[] = { }; static const char * const cms_FilterType[] = { - "PT1", "BIQUAD", "PT2", "PT3", "PT4", + "PT1", "BIQUAD", "PT2", "PT3", "PT4", +#ifdef USE_LULU + "LULU", +#endif }; #ifdef USE_GYRO_DATA_ANALYSE @@ -159,6 +165,9 @@ static long cmsx_PidAdvancedRead(void) { itermWindup = pidProfile->itermWindupPointPercent; emuGravityGain = pidProfile->emuGravityGain; linear_thrust_low_output = pidProfile->linear_thrust_low_output; +#ifdef USE_LULU + lulu_n_val = pidProfile->lulu_n_val; +#endif linear_thrust_high_output = pidProfile->linear_thrust_high_output; linear_throttle = pidProfile->linear_throttle; mixer_impl = pidProfile->mixer_impl; @@ -192,6 +201,9 @@ static long cmsx_PidAdvancedWriteback(const OSD_Entry *self) { pidProfile->itermWindupPointPercent = itermWindup; pidProfile->emuGravityGain = emuGravityGain; pidProfile->linear_thrust_low_output = linear_thrust_low_output; +#ifdef USE_LULU + pidProfile->lulu_n_val = lulu_n_val; +#endif pidProfile->linear_thrust_high_output = linear_thrust_high_output; pidProfile->linear_throttle = linear_throttle; pidProfile->mixer_impl = mixer_impl; @@ -230,7 +242,9 @@ static OSD_Entry cmsx_menuPidAdvancedEntries[] = { { "MIXER IMPL", OME_TAB, NULL, &(OSD_TAB_t) { &mixer_impl, MIXER_IMPL_COUNT - 1, cms_mixerImplTypeLabels }, 0 }, { "MIXER LAZINESS", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &mixer_laziness, 1, cms_offOnLabels }, 0 }, { "MIXER YAW THR COMP", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &mixer_yaw_throttle_comp, 1, cms_offOnLabels }, 0 }, - +#ifdef USE_LULU + { "LULU N", OME_UINT8, NULL, &(OSD_UINT8_t) { &lulu_n_val, 0, 25, 1}, 0 }, +#endif { "SAVE&EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVE, 0}, { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } @@ -788,11 +802,11 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) { static OSD_Entry cmsx_menuFilterPerProfileEntries[] = { { "-- FILTER PP --", OME_Label, NULL, NULL, 0 }, - { "DTERM LPF TYPE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &cmsx_dterm_lowpass_type, 4, cms_FilterType }, 0 }, + { "DTERM LPF TYPE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &cmsx_dterm_lowpass_type, 5, cms_FilterType }, 0 }, { "DTERM LPF ROLL", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass_hz_roll, 0, 500, 1 }, 0 }, { "DTERM LPF PITCH", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass_hz_pitch, 0, 500, 1 }, 0 }, { "DTERM LPF YAW", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass_hz_yaw, 0, 500, 1 }, 0 }, - { "DTERM LPF2 TYPE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &cmsx_dterm_lowpass2_type, 4, cms_FilterType }, 0 }, + { "DTERM LPF2 TYPE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &cmsx_dterm_lowpass2_type, 5, cms_FilterType }, 0 }, { "DTERM LPF2 ROLL", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz_roll, 0, 500, 1 }, 0 }, { "DTERM LPF2 PITCH", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz_pitch, 0, 500, 1 }, 0 }, { "DTERM LPF2 YAW", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz_yaw, 0, 500, 1 }, 0 }, diff --git a/src/main/common/filter.h b/src/main/common/filter.h index ea81c5d284..ed6e03bc5f 100755 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -61,6 +61,9 @@ typedef enum { FILTER_PT2, FILTER_PT3, FILTER_PT4, +#ifdef USE_LULU + FILTER_LULU, +#endif } lowpassFilterType_e; typedef enum { diff --git a/src/main/common/lulu.c b/src/main/common/lulu.c new file mode 100644 index 0000000000..285e392018 --- /dev/null +++ b/src/main/common/lulu.c @@ -0,0 +1,129 @@ +#include "lulu.h" + +#include +#include +#include +#include + +#include "platform.h" + +#include "common/filter.h" +#include "common/maths.h" +#include "common/utils.h" + +#ifdef __ARM_ACLE +#include +#endif /* __ARM_ACLE */ +#include + +void luluFilterInit(luluFilter_t *filter, int N) { + if (N > 15) { + N = 15; + } + if (N < 1) { + N = 1; + } + filter->N = N; + filter->windowSize = filter->N * 2 + 1; + filter->windowBufIndex = 0; + + memset(filter->luluInterim, 0, sizeof(float) * (filter->windowSize)); + memset(filter->luluInterimB, 0, sizeof(float) * (filter->windowSize)); +} + +FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, int windowSize) { + register float curVal = 0; + register float curValB = 0; + for (int N = 1; N <= filterN; N++) { + int indexNeg = (index + windowSize - 2 * N) % windowSize; + register int curIndex = (indexNeg + 1) % windowSize; + register float prevVal = series[indexNeg]; + register float prevValB = seriesB[indexNeg]; + register int indexPos = (curIndex + N) % windowSize; + for (int i = windowSize - 2 * N; i < windowSize - N; i++) { + if (indexPos >= windowSize) { + indexPos = 0; + } + if (curIndex >= windowSize) { + curIndex = 0; + } + + curVal = series[curIndex]; + curValB = seriesB[curIndex]; + register float nextVal = series[indexPos]; + register float nextValB = seriesB[indexPos]; + + if (prevVal < curVal && curVal > nextVal) { + float maxValue = MAX(prevVal, nextVal); + series[curIndex] = maxValue; + } + + if (prevValB < curValB && curValB > nextValB) { + float maxValue = MAX(prevValB, nextValB); + seriesB[curIndex] = maxValue; + } + prevVal = curVal; + prevValB = curValB; + curIndex++; + indexPos++; + } + + curIndex = (indexNeg + 1) % windowSize; + prevVal = series[indexNeg]; + prevValB = seriesB[indexNeg]; + indexPos = (curIndex + N) % windowSize; + for (int i = windowSize - 2 * N; i < windowSize - N; i++) { + if (indexPos >= windowSize) { + indexPos = 0; + } + if (curIndex >= windowSize) { + curIndex = 0; + } + + curVal = series[curIndex]; + curValB = seriesB[curIndex]; + register float nextVal = series[indexPos]; + register float nextValB = seriesB[indexPos]; + + if (prevVal > curVal && curVal < nextVal) { + float minValue = MIN(prevVal, nextVal); + series[curIndex] = minValue; + } + + if (prevValB > curValB && curValB < nextValB) { + float minValue = MIN(prevValB, nextValB); + seriesB[curIndex] = minValue; + } + prevVal = curVal; + prevValB = curValB; + curIndex++; + indexPos++; + } + } + int finalIndex = (index + windowSize - filterN) % windowSize; + curVal = series[finalIndex]; + curValB = seriesB[finalIndex]; + return (curVal - curValB) / 2; +} + +FAST_CODE float luluFilterPartialApply(luluFilter_t *filter, float input) { + // This is the value N of the LULU filter. + register int filterN = filter->N; + // This is the total window size for the rolling buffer + register int filterWindow = filter->windowSize; + + register int windowIndex = filter->windowBufIndex; + register float inputVal = input; + register int newIndex = (windowIndex + 1) % filterWindow; + filter->windowBufIndex = newIndex; + filter->luluInterim[windowIndex] = inputVal; + filter->luluInterimB[windowIndex] = -inputVal; + return fixRoad(filter->luluInterim, filter->luluInterimB, windowIndex, filterN, filterWindow); +} + +FAST_CODE float luluFilterApply(luluFilter_t *filter, float input) { + // This is the UL filter + float resultA = luluFilterPartialApply(filter, input); + // We use the median interpretation of this filter to remove bias in the output + return resultA; +} diff --git a/src/main/common/lulu.h b/src/main/common/lulu.h new file mode 100644 index 0000000000..96c64949d2 --- /dev/null +++ b/src/main/common/lulu.h @@ -0,0 +1,13 @@ +#pragma once + +//Max N = 15 +typedef struct { + int windowSize; + int windowBufIndex; + int N; + float luluInterim[32] __attribute__ ((aligned (128))); + float luluInterimB[32]; +} luluFilter_t; + +void luluFilterInit(luluFilter_t *filter, int N); +float luluFilterApply(luluFilter_t *filter, float input); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 85a073252a..b03b4e0e1f 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -33,6 +33,9 @@ #include "common/filter.h" #include "common/maths.h" #include "common/utils.h" +#ifdef USE_LULU +#include "common/lulu.h" +#endif #include "config/feature.h" #include "pg/pg.h" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e5e7601ab6..311443621d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -31,6 +31,9 @@ #include "common/axis.h" #include "common/maths.h" #include "common/filter.h" +#ifdef USE_LULU +#include "common/lulu.h" +#endif #include "config/config_reset.h" #include "pg/pg.h" @@ -197,6 +200,9 @@ void resetPidProfile(pidProfile_t *pidProfile) { .angle_filter = 100, .dtermDynNotch = false, .dterm_dyn_notch_q = 400, +#ifdef USE_LULU + .lulu_n_val = 2 +#endif ); } @@ -222,6 +228,7 @@ typedef union dtermLowpass_u { pt1Filter_t pt1Filter; biquadFilter_t biquadFilter; ptnFilter_t ptnFilter; + luluFilter_t luluFilter; } dtermLowpass_t; static FAST_RAM_ZERO_INIT float previousPidSetpoint[XYZ_AXIS_COUNT]; @@ -274,6 +281,10 @@ void pidInitFilters(const pidProfile_t *pidProfile) { dtermLowpassApplyFn = (filterApplyFnPtr)ptnFilterApply; ptnFilterInit(&dtermLowpass[axis].ptnFilter, FILTER_PT2, pidProfile->dFilter[axis].dLpf, dT); break; + case FILTER_LULU: + dtermLowpassApplyFn = (filterApplyFnPtr)luluFilterApply; + luluFilterInit(&dtermLowpass[axis].luluFilter, pidProfile->lulu_n_val); + break; default: // case FILTER_PT1: dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply; pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(pidProfile->dFilter[axis].dLpf, dT)); @@ -299,6 +310,10 @@ void pidInitFilters(const pidProfile_t *pidProfile) { dtermLowpass2ApplyFn = (filterApplyFnPtr)ptnFilterApply; ptnFilterInit(&dtermLowpass2[axis].ptnFilter, FILTER_PT2, pidProfile->dFilter[axis].dLpf2, dT); break; + case FILTER_LULU: + dtermLowpass2ApplyFn = (filterApplyFnPtr)luluFilterApply; + luluFilterInit(&dtermLowpass2[axis].luluFilter, pidProfile->lulu_n_val); + break; default: // case FILTER_PT1: dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply; pt1FilterInit(&dtermLowpass2[axis].pt1Filter, pt1FilterGain(pidProfile->dFilter[axis].dLpf2, dT)); @@ -823,8 +838,16 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an } } #endif + if(axis < 2) + { + DEBUG_SET(DEBUG_LULU, axis, lrintf(dDelta)); + } dDelta = dtermLowpassApplyFn((filter_t *)&dtermLowpass[axis], dDelta); dDelta = dtermLowpass2ApplyFn((filter_t *)&dtermLowpass2[axis], dDelta); + if(axis < 2) + { + DEBUG_SET(DEBUG_LULU, axis + 2, lrintf(dDelta)); + } dDelta = dtermABGapplyFn((filter_t *)&dtermABG[axis], dDelta); //dterm boost, similar to emuboost float boostedDtermRate; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index ea9f16d811..1dc5b954d9 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -157,6 +157,9 @@ typedef struct pidProfile_s { uint8_t dterm_ABG_half_life; uint8_t dtermDynNotch; uint16_t dterm_dyn_notch_q; +#ifdef USE_LULU + uint8_t lulu_n_val; +#endif } pidProfile_t; #ifndef USE_OSD_SLAVE diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 3b7d50938c..bc65321836 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -296,6 +296,9 @@ static const char * const lookupTableRcInterpolationChannels[] = { static const char * const lookupTableFilterType[] = { "PT1", "BIQUAD", "PT2", "PT3", "PT4", +#ifdef USE_LULU + "LULU", +#endif }; #if defined(USE_GYRO_IMUF9001) @@ -974,6 +977,9 @@ const clivalue_t valueTable[] = { { "mixer_impl", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MIXER_IMPL_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, mixer_impl) }, { "mixer_laziness", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, mixer_laziness) }, { "mixer_yaw_throttle_comp", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, mixer_yaw_throttle_comp) }, +#ifdef USE_LULU + { "lulu_n_val", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 25 }, PG_PID_PROFILE, offsetof(pidProfile_t, lulu_n_val) }, +#endif // PG_TELEMETRY_CONFIG diff --git a/src/main/io/osd.c b/src/main/io/osd.c index c13d3da510..902f157369 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -219,7 +219,10 @@ static const uint8_t osdElementDisplayOrder[] = { OSD_DISARMED, OSD_NUMERICAL_HEADING, OSD_NUMERICAL_VARIO, - OSD_COMPASS_BAR + OSD_COMPASS_BAR, +#ifdef USE_LULU + OSD_LULU_N, +#endif }; PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 5); @@ -798,6 +801,9 @@ static bool osdDrawSingleElement(uint8_t item) { tfp_sprintf(buff, "%01d.%01dG", (int)osdGForce, (int)(osdGForce * 10) % 10); break; } + case OSD_LULU_N: + tfp_sprintf(buff, "N=%2d", ¤tPidProfile->lulu_n_val); + break; case OSD_ROLL_PIDS: osdFormatPID(buff, "ROL", ¤tPidProfile->pid[PID_ROLL]); break; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 15ffb20cc6..c852a62180 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -111,6 +111,9 @@ typedef enum { OSD_CRSF_RSSI, OSD_MAH_PERCENT, OSD_PLUS_CODE, +#ifdef USE_LULU + OSD_LULU_N, +#endif OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 1da8a2399c..d2af2e74c8 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -228,6 +228,7 @@ #define USE_SIGNATURE #define USE_CMS_FAILSAFE_MENU #define USE_CMS_GPS_RESCUE_MENU +#define USE_LULU #endif // ICM42688P & BMI270 experimental define