From 0b8b37f6725c1891ba4d3da230befa71577cf6a4 Mon Sep 17 00:00:00 2001 From: rob Date: Wed, 26 Jun 2019 18:40:42 +0200 Subject: [PATCH] wip for today. --- src/main/build/version.h | 2 +- src/main/cms/cms_menu_imu.c | 23 +++- src/main/common/filter.c | 117 +++++++++++++++----- src/main/common/filter.h | 28 +++-- src/main/config/config_eeprom.h | 2 +- src/main/drivers/accgyro/accgyro_imuf9001.c | 3 +- src/main/fc/config.c | 2 +- src/main/fc/fc_rc.c | 6 +- src/main/fc/fc_rc.h | 6 +- src/main/flight/imu.h | 12 +- src/main/flight/pid.c | 36 ++++-- src/main/flight/pid.h | 2 + src/main/interface/msp.c | 4 +- src/main/interface/settings.c | 5 +- src/main/sensors/acceleration.c | 21 ++-- src/main/sensors/gyro.c | 17 ++- src/main/sensors/gyro.h | 4 +- src/main/sensors/gyro_filter_impl.h | 4 +- src/main/sensors/sensors.h | 2 +- src/main/target/HELIOSPRING/target.h | 5 +- src/main/target/STRIXF10/target.h | 1 + support/buildserver/upload.php | 114 +++++++++---------- 22 files changed, 263 insertions(+), 153 deletions(-) mode change 100644 => 100755 src/main/common/filter.c diff --git a/src/main/build/version.h b/src/main/build/version.h index 30f5158c0..0733bf233 100644 --- a/src/main/build/version.h +++ b/src/main/build/version.h @@ -25,7 +25,7 @@ #define FC_FIRMWARE_NAME "ButterFlight" #define FC_VERSION_MAJOR 3 // increment when a major release is made (big new feature, etc) #define FC_VERSION_MINOR 6 // increment when a minor release is made (small new feature, change etc) -#define FC_VERSION_PATCH_LEVEL 6 // increment when a bug is fixed +#define FC_VERSION_PATCH_LEVEL 7 // increment when a bug is fixed #define FC_VERSION_STRING STR(FC_VERSION_MAJOR) "." STR(FC_VERSION_MINOR) "." STR(FC_VERSION_PATCH_LEVEL) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index cd3407e7d..3609fc340 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -58,6 +58,8 @@ static uint8_t tmpPidProfileIndex; static uint8_t pidProfileIndex; static char pidProfileIndexString[] = " p"; static uint8_t buttered_pids; +static uint8_t i_decay; +static uint8_t r_weight; static uint8_t tempPid[3][3]; static uint16_t tempPidF[3]; @@ -119,6 +121,8 @@ static long cmsx_PidRead(void) const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex); buttered_pids = pidProfile->buttered_pids; + i_decay = pidProfile->i_decay; + r_weight = pidProfile->r_weight; for (uint8_t i = 0; i < 3; i++) { tempPid[i][0] = pidProfile->pid[i].P; tempPid[i][1] = pidProfile->pid[i].I; @@ -149,6 +153,8 @@ static long cmsx_PidWriteback(const OSD_Entry *self) pidProfile->pid[i].D = tempPid[i][2]; pidProfile->pid[i].F = tempPidF[i]; } + pidProfile->i_decay = i_decay; + pidProfile->r_weight = r_weight; pidInitConfig(currentPidProfile); return 0; @@ -174,6 +180,9 @@ static OSD_Entry cmsx_menuPidEntries[] = { "YAW D", OME_UINT8, NULL, &(OSD_UINT8_t){ &tempPid[PID_YAW][2], 0, 200, 1 }, 0 }, { "YAW F", OME_UINT16, NULL, &(OSD_UINT16_t){ &tempPidF[PID_YAW], 0, 2000, 1 }, 0 }, + { "I_DECAY", OME_UINT8, NULL, &(OSD_UINT8_t){ &i_decay, 1, 10, 1 }, 0 }, + { "R_WEIGHT", OME_UINT8, NULL, &(OSD_UINT8_t){ &r_weight, 1, 200, 1 }, 0 }, + { "BACK", OME_Back, NULL, NULL, 0 }, { "SAVE&EXIT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVE, 0}, { NULL, OME_END, NULL, NULL, 0 } @@ -337,7 +346,7 @@ static uint16_t gyroConfig_gyro_soft_notch_hz_2; static uint16_t gyroConfig_gyro_soft_notch_cutoff_2; #ifndef USE_GYRO_IMUF9001 static uint16_t gyroConfig_gyro_filter_q; -static uint16_t gyroConfig_gyro_filter_r; +static uint16_t gyroConfig_gyro_filter_w; #endif static long cmsx_menuGyro_onEnter(void) @@ -351,7 +360,7 @@ static long cmsx_menuGyro_onEnter(void) #ifndef USE_GYRO_IMUF9001 gyroConfig_gyro_filter_q = gyroConfig()->gyro_filter_q; - gyroConfig_gyro_filter_r = gyroConfig()->gyro_filter_r; + gyroConfig_gyro_filter_w = gyroConfig()->gyro_filter_w; #endif return 0; } @@ -369,7 +378,7 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) #ifndef USE_GYRO_IMUF9001 gyroConfigMutable()->gyro_filter_q = gyroConfig_gyro_filter_q; - gyroConfigMutable()->gyro_filter_r = gyroConfig_gyro_filter_r; + gyroConfigMutable()->gyro_filter_w = gyroConfig_gyro_filter_w; #endif return 0; } @@ -388,7 +397,7 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] = { "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 }, 0 }, #ifndef USE_GYRO_IMUF9001 { "KALMAN Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_filter_q, 0, 16000, 1 }, 0 }, - { "KALMAN R", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_filter_r, 0, 16000, 1 }, 0 }, + { "KALMAN W", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_filter_w, 3, 64, 1 }, 0 }, #endif { "BACK", OME_Back, NULL, NULL, 0 }, { NULL, OME_END, NULL, NULL, 0 } @@ -418,6 +427,7 @@ static uint16_t gyroConfig_imuf_w; static uint16_t gyroConfig_imuf_pitch_lpf_cutoff_hz; static uint16_t gyroConfig_imuf_roll_lpf_cutoff_hz; static uint16_t gyroConfig_imuf_yaw_lpf_cutoff_hz; +static uint16_t gyroConfig_imuf_acc_lpf_cutoff_hz; #endif #if defined(USE_GYRO_IMUF9001) @@ -430,6 +440,7 @@ static long cmsx_menuImuf_onEnter(void) gyroConfig_imuf_pitch_lpf_cutoff_hz = gyroConfig()->imuf_pitch_lpf_cutoff_hz; gyroConfig_imuf_roll_lpf_cutoff_hz = gyroConfig()->imuf_roll_lpf_cutoff_hz; gyroConfig_imuf_yaw_lpf_cutoff_hz = gyroConfig()->imuf_yaw_lpf_cutoff_hz; + gyroConfig_imuf_acc_lpf_cutoff_hz = gyroConfig()->imuf_acc_lpf_cutoff_hz; return 0; } @@ -447,6 +458,7 @@ static long cmsx_menuImuf_onExit(const OSD_Entry *self) gyroConfigMutable()->imuf_roll_lpf_cutoff_hz = gyroConfig_imuf_roll_lpf_cutoff_hz; gyroConfigMutable()->imuf_pitch_lpf_cutoff_hz = gyroConfig_imuf_pitch_lpf_cutoff_hz; gyroConfigMutable()->imuf_yaw_lpf_cutoff_hz = gyroConfig_imuf_yaw_lpf_cutoff_hz; + gyroConfigMutable()->imuf_acc_lpf_cutoff_hz = gyroConfig_imuf_acc_lpf_cutoff_hz; return 0; } #endif @@ -463,8 +475,9 @@ static OSD_Entry cmsx_menuImufEntries[] = { "ROLL LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_lpf_cutoff_hz, 0, 450, 1 }, 0 }, { "PITCH LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_pitch_lpf_cutoff_hz, 0, 450, 1 }, 0 }, { "YAW LPF", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_yaw_lpf_cutoff_hz, 0, 450, 1 }, 0 }, + { "IMUF ACC", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_acc_lpf_cutoff_hz, 0, 450, 1 }, 0 }, - { "BACK", OME_Back, NULL, NULL, 0}, + { "BACK", OME_Back, NULL, NULL, 0}, { "SAVE&REBOOT", OME_OSD_Exit, cmsMenuExit, (void *)CMS_EXIT_SAVEREBOOT, 0}, { NULL, OME_END, NULL, NULL, 0 } }; diff --git a/src/main/common/filter.c b/src/main/common/filter.c old mode 100644 new mode 100755 index 1751b47da..caf1cfb05 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -29,9 +29,15 @@ #include "common/maths.h" #include "common/utils.h" -#define M_LN2_FLOAT 0.69314718055994530942f -#define M_PI_FLOAT 3.14159265358979323846f -#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - 2nd order butterworth*/ +#include "fc/fc_rc.h" + +#define M_LN2_FLOAT 0.69314718055994530942f +#define M_PI_FLOAT 3.14159265358979323846f +#define BIQUAD_Q (1.0f / sqrtf(2.0f)) /* quality factor - 2nd order butterworth*/ + +#define BASE_LPF_HZ 70.0f + +float r_weight = 0.67f; // NULL filter @@ -46,7 +52,7 @@ FAST_CODE float nullFilterApply(filter_t *filter, float input) float pt1FilterGain(uint16_t f_cut, float dT) { - float RC = 1 / ( 2 * M_PI_FLOAT * f_cut); + const float RC = 0.5f / (M_PI_FLOAT * f_cut); return dT / (RC + dT); } @@ -215,48 +221,101 @@ void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, } // Proper fast two-state Kalman -void fastKalmanInit(fastKalman_t *filter, float q, float r) +void fastKalmanInit(fastKalman_t *filter, float q, uint32_t w, int axis, float updateRate) { + if ( w > 64) + { + w = 64; + } + + memset(filter, 0, sizeof(fastKalman_t)); filter->q = q * 0.000001f; // add multiplier to make tuning easier - filter->r = r * 0.001f; // add multiplier to make tuning easier filter->p = q * 0.001f; // add multiplier to make tuning easier - filter->x = 0.0f; // set initial value, can be zero if unknown - filter->lastX = 0.0f; // set initial value, can be zero if unknown - filter->k = 0.0f; // kalman gain + filter->w = w; + filter->windowSizeInverse = 1.0f/(w - 1); + filter->axis = axis; + + // set cutoff frequency + const float k = pt1FilterGain(BASE_LPF_HZ, updateRate); + pt1FilterInit(&filter->lp_filter, k); + filter->lp_filter.state = 1.0f; // e's default value + filter->updateRate = updateRate; } -#ifndef STM32F7 -FAST_CODE float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input) -{ - filter->movingSum -= filter->buf[filter->movingWindowIndex]; - filter->buf[filter->movingWindowIndex] = input; - filter->movingSum += input; - - if (++filter->movingWindowIndex == filter->windowSize) { - filter->movingWindowIndex = 0; - filter->primed = true; - } - - const uint16_t denom = filter->primed ? filter->windowSize : filter->movingWindowIndex; - return filter->movingSum / denom; -} -#endif +#pragma GCC push_options +#pragma GCC optimize("O3") FAST_CODE float fastKalmanUpdate(fastKalman_t *filter, float input) { + static float e; + + const float setPoint = getSetpointRate(filter->axis); + const float filteredValue = filter->x; + // project the state ahead using acceleration filter->x += (filter->x - filter->lastX); // update last state filter->lastX = filter->x; + // figure out how much to boost or reduce our error in the estimate based on setPoint target. + // this should be close to 0 as we approach the setPoint and really high the further away we are from the setPoint. + if (setPoint != 0.0f && filteredValue != 0.0f) + { + e = ABS(1.0f - (setPoint/filteredValue)); + } + else + { + e = 1.0f; + } + //e = pt1FilterApply(&filter->lp_filter, e); + // prediction update - filter->p = filter->p + filter->q; + filter->p = filter->p + filter->q * e; // measurement update - filter->k = filter->p / (filter->p + filter->r); - filter->x += filter->k * (input - filter->x); - filter->p = (1.0f - filter->k) * filter->p; + const float k = filter->p / (filter->p + filter->r); + filter->x += k * (input - filter->x); + filter->p = (1.0f - k) * filter->p; + + filter->x = pt1FilterApply(&filter->lp_filter, filter->x); + + // update variance + filter->window[filter->windowIndex] = input; + + filter->meanSum += filter->window[filter->windowIndex]; + filter->varianceSum = filter->varianceSum + (filter->window[filter->windowIndex] * filter->window[filter->windowIndex]); + + filter->windowIndex++; + if (filter->windowIndex >= filter->w) + { + filter->windowIndex = 0; + } + + filter->meanSum -= filter->window[filter->windowIndex]; + filter->varianceSum = filter->varianceSum - (filter->window[filter->windowIndex] * filter->window[filter->windowIndex]); + + filter->mean = filter->meanSum * filter->windowSizeInverse; + filter->variance = ABS(filter->varianceSum * filter->windowSizeInverse - (filter->mean * filter->mean)); + filter->r = sqrtf(filter->variance) * r_weight; + + + if (isSetpointNew) + { + if (setPoint != 0.0f && filter->oldSetPoint != setPoint) + { + const float cutoff_frequency = constrain(BASE_LPF_HZ * e, 10.0f, 500.0f); + const float k = pt1FilterGain(cutoff_frequency, filter->updateRate); + pt1FilterUpdateCutoff(&filter->lp_filter, k); + filter->oldSetPoint = setPoint; + } + if (filter->axis == 2) + { + isSetpointNew = false; + } + } return filter->x; } + +#pragma GCC pop_options diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 4f275a9d4..d581ca090 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -61,13 +61,31 @@ typedef enum { FILTER_BPF, } biquadFilterType_e; -typedef struct fastKalman_s { +#if (defined(STM32F7) || defined(STM32F4)) +#define MAX_WINDOW_SIZE 256 +#else +#define MAX_WINDOW_SIZE 64 +#endif + +typedef struct kalman_s { + uint32_t w; // window size float q; // process noise covariance float r; // measurement noise covariance float p; // estimation error covariance matrix - float k; // kalman gain float x; // state float lastX; // previous state + + float window[MAX_WINDOW_SIZE]; + float variance; + float varianceSum; + float mean; + float meanSum; + float windowSizeInverse; + uint32_t windowIndex; + int axis; // for setPoint not being passed during call time + pt1Filter_t lp_filter; + float oldSetPoint; + float updateRate; } fastKalman_t; typedef float (*filterApplyFnPtr)(filter_t *filter, float input); @@ -82,10 +100,6 @@ void biquadFilterUpdateLPF(biquadFilter_t *filter, float filterFreq, uint32_t re float biquadFilterApplyDF1(biquadFilter_t *filter, float input); float biquadFilterApply(biquadFilter_t *filter, float input); float filterGetNotchQ(float centerFreq, float cutoffFreq); -#ifndef STM32F7 -void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf); -float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input); -#endif float pt1FilterGain(uint16_t f_cut, float dT); void pt1FilterInit(pt1Filter_t *filter, float k); void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k); @@ -94,5 +108,5 @@ float pt1FilterApply(pt1Filter_t *filter, float input); void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold); float slewFilterApply(slewFilter_t *filter, float input); -void fastKalmanInit(fastKalman_t *filter, float q, float r); +void fastKalmanInit(fastKalman_t *filter, float q, uint32_t w, int axis, float updateRate); float fastKalmanUpdate(fastKalman_t *filter, float input); diff --git a/src/main/config/config_eeprom.h b/src/main/config/config_eeprom.h index 0c3364c2d..07c6f2490 100644 --- a/src/main/config/config_eeprom.h +++ b/src/main/config/config_eeprom.h @@ -23,7 +23,7 @@ #include #include -#define EEPROM_CONF_VERSION 171 +#define EEPROM_CONF_VERSION 173 bool isEEPROMVersionValid(void); bool isEEPROMStructureValid(void); diff --git a/src/main/drivers/accgyro/accgyro_imuf9001.c b/src/main/drivers/accgyro/accgyro_imuf9001.c index 088237a24..8b9cefe0c 100644 --- a/src/main/drivers/accgyro/accgyro_imuf9001.c +++ b/src/main/drivers/accgyro/accgyro_imuf9001.c @@ -577,6 +577,7 @@ void setupImufParams(imufCommand_t * data) data->param7 = ( (uint16_t)0 << 16) | (uint16_t)0; data->param8 = ( (int16_t)boardAlignment()->rollDegrees << 16 ) | imufGyroAlignment(); data->param9 = ( (int16_t)boardAlignment()->yawDegrees << 16 ) | (int16_t)boardAlignment()->pitchDegrees; + data->param10 = ( (uint16_t)gyroConfig()->r_weight << 16) | ( (int16_t)gyroConfig()->imuf_acc_lpf_cutoff_hz ); } } @@ -647,4 +648,4 @@ FAST_CODE void imufEndCalibration(void) isImufCalibrating = IMUF_NOT_CALIBRATING; //reset by EXTI } -#endif \ No newline at end of file +#endif diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 891de7073..b8ea28dcc 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -422,13 +422,13 @@ int getImufRateFromGyroSyncDenom(int gyroSyncDenom){ return IMUF_RATE_32K; break; case 2: - default: return IMUF_RATE_16K; break; case 4: return IMUF_RATE_8K; break; case 8: + default: return IMUF_RATE_4K; break; case 16: diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index ecbb40c1e..dcde46475 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -60,9 +60,7 @@ enum { THROTTLE_FLAG = 1 << THROTTLE, }; -#ifdef USE_GYRO_IMUF9001 - volatile bool isSetpointNew; -#endif +volatile bool isSetpointNew; typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs); @@ -651,9 +649,7 @@ FAST_CODE void processRcCommand(void) } DEBUG_SET(DEBUG_RC_INTERPOLATION, 3, setpointRate[0]); -#ifdef USE_GYRO_IMUF9001 isSetpointNew = 1; -#endif if (debugMode == DEBUG_RC_INTERPOLATION) { debug[2] = rcInterpolationStepCount; debug[3] = setpointRate[0]; diff --git a/src/main/fc/fc_rc.h b/src/main/fc/fc_rc.h index ac488047d..a14433072 100644 --- a/src/main/fc/fc_rc.h +++ b/src/main/fc/fc_rc.h @@ -28,10 +28,8 @@ typedef enum { INTERPOLATION_CHANNELS_RPT, } interpolationChannels_e; -#ifdef USE_GYRO_IMUF9001 -extern volatile bool isSetpointNew; -#endif -extern volatile uint16_t currentRxRefreshRate; +extern volatile bool isSetpointNew; +extern volatile uint16_t currentRxRefreshRate; void processRcCommand(void); float getSetpointRate(int axis); diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 9509a978e..0d9567aff 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -26,15 +26,15 @@ #include "pg/pg.h" #ifndef DEFAULT_ATTITUDE_UPDATE_INTERVAL -#define DEFAULT_ATTITUDE_UPDATE_INTERVAL 200 +#define DEFAULT_ATTITUDE_UPDATE_INTERVAL 400 // up from 200 #endif //DEFAULT_ATTITUDE_UPDATE_INTERVAL 200 // Exported symbols -extern uint32_t accTimeSum; -extern int accSumCount; -extern float accVelScale; -extern int32_t accSum[XYZ_AXIS_COUNT]; -extern float accAverage[XYZ_AXIS_COUNT]; +extern uint32_t accTimeSum; +extern int accSumCount; +extern float accVelScale; +extern int32_t accSum[XYZ_AXIS_COUNT]; +extern float accAverage[XYZ_AXIS_COUNT]; typedef union { int16_t raw[XYZ_AXIS_COUNT]; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e53d68c74..b88036bdb 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -56,6 +56,8 @@ #include "sensors/acceleration.h" +extern float r_weight; + #define ITERM_RELAX_SETPOINT_THRESHOLD 30.0f const char pidNames[] = @@ -96,11 +98,11 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2); #endif //USE_BUTTERED_PIDS #ifndef DEFAULT_PIDS_ROLL -#define DEFAULT_PIDS_ROLL { 40, 40, 20, 60 } +#define DEFAULT_PIDS_ROLL { 44, 55, 28, 60 } #endif //DEFAULT_PIDS_ROLL #ifndef DEFAULT_PIDS_PITCH -#define DEFAULT_PIDS_PITCH { 58, 50, 22, 60 } +#define DEFAULT_PIDS_PITCH { 58, 60, 30, 60 } #endif //DEFAULT_PIDS_PITCH #ifndef DEFAULT_PIDS_YAW @@ -131,6 +133,8 @@ PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG void resetPidProfile(pidProfile_t *pidProfile) { + r_weight = 0.67; + RESET_CONFIG(pidProfile_t, pidProfile, .pid = { [PID_ROLL] = DEFAULT_PIDS_ROLL, @@ -141,18 +145,20 @@ void resetPidProfile(pidProfile_t *pidProfile) }, .pidSumLimit = PIDSUM_LIMIT_MAX, - .yaw_lowpass_hz = 0, + .yaw_lowpass_hz = 30, .dterm_lowpass_hz = 65, // filtering ON by default .dterm_lowpass2_hz = 200, // second Dterm LPF ON by default - .dterm_notch_hz = 260, - .dterm_notch_cutoff = 160, - .dterm_filter_type = FILTER_BIQUAD, + .dterm_notch_hz = 0, + .dterm_notch_cutoff = 0, + .dterm_filter_type = FILTER_PT1, .itermWindupPointPercent = 50, .vbatPidCompensation = 0, .pidAtMinThrottle = PID_STABILISATION_ON, .levelAngleLimit = 55, .feedForwardTransition = 0, .buttered_pids = USE_BUTTERED_PIDS, + .i_decay = 2, + .r_weight = 67, .yawRateAccelLimit = 100, .rateAccelLimit = 0, .itermThrottleThreshold = 350, @@ -194,6 +200,8 @@ void pgResetFn_pidProfiles(pidProfile_t *pidProfiles) } } +extern void init_pwm_filter(float dT); + static void pidSetTargetLooptime(uint32_t pidLooptime) { targetPidLooptime = pidLooptime; @@ -253,6 +261,8 @@ void pidInitFilters(const pidProfile_t *pidProfile) dtermLowpassApplyFn = nullFilterApply; const uint32_t pidFrequencyNyquist = pidFrequency / 2; // No rounding needed + r_weight = (float) pidProfile->r_weight / 100.0f; + uint16_t dTermNotchHz; if (pidProfile->dterm_notch_hz <= pidFrequencyNyquist) { dTermNotchHz = pidProfile->dterm_notch_hz; @@ -835,7 +845,6 @@ static FAST_RAM_ZERO_INIT timeUs_t crashDetectedAtUs; // Butterflight pid controller which uses measurement instead of error rate to calculate D FAST_CODE float butteredPids(const pidProfile_t *pidProfile, int axis, float errorRate, float dynCi, float iDT, float currentPidSetpoint) { - (void)(pidProfile); (void) iDT; (void)(currentPidSetpoint); // -----calculate P component @@ -849,7 +858,11 @@ FAST_CODE float butteredPids(const pidProfile_t *pidProfile, int axis, float err { if (SIGN(iterm) != SIGN(ITermNew)) { - iterm *= 0.8f; + const float newVal = ITermNew * (float)pidProfile->i_decay; + if (fabs(iterm) > fabs(newVal)) + { + ITermNew = newVal; + } } } @@ -880,7 +893,6 @@ FAST_CODE float butteredPids(const pidProfile_t *pidProfile, int axis, float err FAST_CODE float classicPids(const pidProfile_t* pidProfile, int axis, float errorRate, float dynCi, float iDT, float currentPidSetpoint) { - UNUSED(pidProfile); UNUSED(iDT); UNUSED(currentPidSetpoint); float gyroRateDterm[XYZ_AXIS_COUNT]; @@ -974,7 +986,11 @@ FAST_CODE float classicPids(const pidProfile_t* pidProfile, int axis, float erro { if (SIGN(ITerm) != SIGN(ITermNew)) { - ITerm *= 0.8f; + const float newVal = ITermNew * (float)pidProfile->i_decay; + if (fabs(ITerm) > fabs(newVal)) + { + ITermNew = newVal; + } } } ITermNew = constrainf(ITerm + ITermNew, -itermLimit, itermLimit); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 292a24aa5..8ce0ff5df 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -117,6 +117,8 @@ typedef struct pidProfile_s { uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms uint16_t itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit uint8_t buttered_pids; // option to use separate pid controller at runtime. + uint8_t i_decay; // i-term decay + uint8_t r_weight; // the weight of the kalman R term calculated out of the std. dev. uint16_t yawRateAccelLimit; // yaw accel limiter for deg/sec/ms uint16_t rateAccelLimit; // accel limiter roll/pitch deg/sec/ms uint16_t crash_dthreshold; // dterm crash value diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index cbfedbde5..cd20aee3d 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1285,7 +1285,7 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) #ifndef USE_GYRO_IMUF9001 case MSP_FAST_KALMAN: sbufWriteU16(dst, gyroConfig()->gyro_filter_q); - sbufWriteU16(dst, gyroConfig()->gyro_filter_r); + sbufWriteU16(dst, gyroConfig()->gyro_filter_w); break; #else case MSP_IMUF_CONFIG: @@ -1854,7 +1854,7 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) #ifndef USE_GYRO_IMUF9001 case MSP_SET_FAST_KALMAN: gyroConfigMutable()->gyro_filter_q = sbufReadU16(src); - gyroConfigMutable()->gyro_filter_r = sbufReadU16(src); + gyroConfigMutable()->gyro_filter_w = sbufReadU16(src); break; #else diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index c53565d9f..056222868 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -521,9 +521,10 @@ const clivalue_t valueTable[] = { { "imuf_pitch_lpf_cutoff_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 450 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_pitch_lpf_cutoff_hz) }, { "imuf_roll_lpf_cutoff_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 450 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_roll_lpf_cutoff_hz) }, { "imuf_yaw_lpf_cutoff_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 450 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_yaw_lpf_cutoff_hz) }, + { "imuf_acc_lpf_cutoff_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 180 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_acc_lpf_cutoff_hz) }, #else { "gyro_filter_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_q) }, - { "gyro_filter_r", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_r) }, + { "gyro_filter_w", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 3, 64 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_w) }, #endif #ifdef USE_GYRO_OVERFLOW_CHECK { "gyro_overflow_detect", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_OVERFLOW_CHECK }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, checkOverflow) }, @@ -873,6 +874,8 @@ const clivalue_t valueTable[] = { #endif // USE_ACRO_TRAINER { "buttered_pids", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, buttered_pids) }, + { "i_decay", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 10 }, PG_PID_PROFILE, offsetof(pidProfile_t, i_decay) }, + { "r_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, r_weight) }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].I) }, { "d_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].D) }, diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index ea3ea1bf3..2c671c5d1 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -100,7 +100,7 @@ extern bool AccInflightCalibrationActive; static flightDynamicsTrims_t *accelerationTrims; static uint16_t accLpfCutHz = 0; -static biquadFilter_t accFilter[XYZ_AXIS_COUNT]; +static pt1Filter_t accFilterPt1[XYZ_AXIS_COUNT]; PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0); @@ -132,7 +132,7 @@ void accResetFlightDynamicsTrims(void) void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) { RESET_CONFIG_2(accelerometerConfig_t, instance, - .acc_lpf_hz = 10, + .acc_lpf_hz = 30, .acc_align = ALIGN_DEFAULT, .acc_hardware = ACC_DEFAULT, .acc_high_fsr = false, @@ -375,9 +375,12 @@ bool accInit(void) acc.dev.acc_1G = 256; // set default acc.dev.initFn(&acc.dev); // driver initialisation // set the acc sampling interval according to the gyro sampling interval - if (accLpfCutHz) { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, DEFAULT_ACC_SAMPLE_INTERVAL); + if (accLpfCutHz) + { + const float k = pt1FilterGain(accLpfCutHz, 1.0f / (float)DEFAULT_ACC_SAMPLE_INTERVAL); + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) + { + pt1FilterInit(&accFilterPt1[axis], k); } } #ifndef USE_ACC_IMUF9001 @@ -510,7 +513,7 @@ void accUpdate(timeUs_t currentTimeUs, rollAndPitchTrims_t *rollAndPitchTrims) if (accLpfCutHz) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - acc.accADC[axis] = biquadFilterApply(&accFilter[axis], (float)acc.accADC[axis]); + acc.accADC[axis] = pt1FilterApply(&accFilterPt1[axis], (float)acc.accADC[axis]); } } @@ -558,8 +561,10 @@ void accInitFilters(void) { accLpfCutHz = accelerometerConfig()->acc_lpf_hz; if (accLpfCutHz) { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, DEFAULT_ACC_SAMPLE_INTERVAL); + const float k = pt1FilterGain(accLpfCutHz, 1.0f / (float)DEFAULT_ACC_SAMPLE_INTERVAL); + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) + { + pt1FilterInit(&accFilterPt1[axis], k); } } } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 43cda95c3..a474100da 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -232,7 +232,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .yaw_spin_recovery = true, .yaw_spin_threshold = 1950, .imuf_mode = GTBCM_DEFAULT, - .imuf_rate = IMUF_RATE_16K, + .imuf_rate = IMUF_RATE_4K, .imuf_roll_q = IMUF_DEFAULT_ROLL_Q, .imuf_pitch_q = IMUF_DEFAULT_PITCH_Q, .imuf_yaw_q = IMUF_DEFAULT_YAW_Q, @@ -240,6 +240,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .imuf_roll_lpf_cutoff_hz = IMUF_DEFAULT_LPF_HZ, .imuf_pitch_lpf_cutoff_hz = IMUF_DEFAULT_LPF_HZ, .imuf_yaw_lpf_cutoff_hz = IMUF_DEFAULT_LPF_HZ, + .imuf_acc_lpf_cutoff_hz = IMUF_DEFAULT_ACC_LPF_HZ, .gyro_offset_yaw = 0, ); #else //USE_GYRO_IMUF9001 @@ -252,7 +253,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL, .gyro_lowpass_type = FILTER_KALMAN, .gyro_lowpass_hz = 0, - .gyro_lowpass2_type = FILTER_BIQUAD, + .gyro_lowpass2_type = FILTER_PT1, .gyro_lowpass2_hz = 90, .gyro_high_fsr = false, .gyro_use_32khz = false, @@ -263,7 +264,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_soft_notch_cutoff_2 = 0, .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES, .gyro_filter_q = 400, - .gyro_filter_r = 88, + .gyro_filter_w = 32, .gyro_offset_yaw = 0, .yaw_spin_recovery = true, .yaw_spin_threshold = 1950, @@ -759,7 +760,7 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint case FILTER_KALMAN: *lowpassFilterApplyFn = (filterApplyFnPtr) fastKalmanUpdate; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - fastKalmanInit(&lowpassFilter[axis].kalmanFilterState, gyroConfig()->gyro_filter_q, gyroConfig()->gyro_filter_r); + fastKalmanInit(&lowpassFilter[axis].kalmanFilterState, gyroConfig()->gyro_filter_q, gyroConfig()->gyro_filter_w, axis, gyroDt); } break; } @@ -1275,9 +1276,9 @@ FAST_CODE_NOINLINE void gyroUpdate(timeUs_t currentTimeUs) gyroUpdateSensor(&gyroSensor1, currentTimeUs); gyroUpdateSensor(&gyroSensor2, currentTimeUs); if (isGyroSensorCalibrationComplete(&gyroSensor1) && isGyroSensorCalibrationComplete(&gyroSensor2)) { - gyro.gyroADCf[X] = (gyroSensor1.gyroDev.gyroADCf[X] + gyroSensor2.gyroDev.gyroADCf[X]) / 2.0f; - gyro.gyroADCf[Y] = (gyroSensor1.gyroDev.gyroADCf[Y] + gyroSensor2.gyroDev.gyroADCf[Y]) / 2.0f; - gyro.gyroADCf[Z] = (gyroSensor1.gyroDev.gyroADCf[Z] + gyroSensor2.gyroDev.gyroADCf[Z]) / 2.0f; + gyro.gyroADCf[X] = (gyroSensor1.gyroDev.gyroADCf[X] + gyroSensor2.gyroDev.gyroADCf[X]) * 0.5f; + gyro.gyroADCf[Y] = (gyroSensor1.gyroDev.gyroADCf[Y] + gyroSensor2.gyroDev.gyroADCf[Y]) * 0.5f; + gyro.gyroADCf[Z] = (gyroSensor1.gyroDev.gyroADCf[Z] + gyroSensor2.gyroDev.gyroADCf[Z]) * 0.5f; #ifdef USE_GYRO_OVERFLOW_CHECK overflowDetected = gyroSensor1.overflowDetected || gyroSensor2.overflowDetected; #endif @@ -1315,8 +1316,6 @@ FAST_CODE_NOINLINE void gyroUpdate(timeUs_t currentTimeUs) #endif #endif - - if (!overflowDetected) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // integrate using trapezium rule to avoid bias diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 32e767367..834984045 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -121,10 +121,12 @@ typedef struct gyroConfig_s { uint16_t imuf_pitch_lpf_cutoff_hz; uint16_t imuf_roll_lpf_cutoff_hz; uint16_t imuf_yaw_lpf_cutoff_hz; + uint16_t imuf_acc_lpf_cutoff_hz; #else uint16_t gyro_filter_q; - uint16_t gyro_filter_r; + uint16_t gyro_filter_w; #endif + uint8_t r_weight; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h index c32d13a09..9e47730b8 100644 --- a/src/main/sensors/gyro_filter_impl.h +++ b/src/main/sensors/gyro_filter_impl.h @@ -17,10 +17,10 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) #endif // apply static notch filters and software lowpass filters + gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); + gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); - gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); - gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); #ifdef USE_GYRO_DATA_ANALYSE if (isDynamicFilterActive()) { diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index d26463efb..d0ab02b88 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -43,7 +43,7 @@ typedef union flightDynamicsTrims_u { flightDynamicsTrims_def_t values; } flightDynamicsTrims_t; -#define CALIBRATING_ACC_CYCLES 400 +#define CALIBRATING_ACC_CYCLES 1000 // upped from 400 #define CALIBRATING_BARO_CYCLES 200 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles typedef enum { diff --git a/src/main/target/HELIOSPRING/target.h b/src/main/target/HELIOSPRING/target.h index 3b4bc6265..d97698380 100644 --- a/src/main/target/HELIOSPRING/target.h +++ b/src/main/target/HELIOSPRING/target.h @@ -201,11 +201,12 @@ #define IMUF_DEFAULT_ROLL_Q 3000 #define IMUF_DEFAULT_YAW_Q 3000 #define IMUF_DEFAULT_W 32 -#define IMUF_DEFAULT_LPF_HZ 120.0f +#define IMUF_DEFAULT_LPF_HZ 70.0f +#define IMUF_DEFAULT_ACC_LPF_HZ 40.0f #define USE_BUTTERED_PIDS true #define DEFAULT_PIDS_ROLL {45, 50, 20, 0} #define DEFAULT_PIDS_PITCH {45, 50, 20, 0} #define DEFAULT_PIDS_YAW {45, 50, 20, 0} -#define TARGET_MANUFACTURER_IDENTIFIER "HESP" \ No newline at end of file +#define TARGET_MANUFACTURER_IDENTIFIER "HESP" diff --git a/src/main/target/STRIXF10/target.h b/src/main/target/STRIXF10/target.h index e5ceec458..9e945c134 100644 --- a/src/main/target/STRIXF10/target.h +++ b/src/main/target/STRIXF10/target.h @@ -194,6 +194,7 @@ #define IMUF_DEFAULT_YAW_Q 3000 #define IMUF_DEFAULT_W 32 #define IMUF_DEFAULT_LPF_HZ 120.0f + #define IMUF_DEFAULT_ACC_LPF_HZ 40.0f #define USE_BUTTERED_PIDS true diff --git a/support/buildserver/upload.php b/support/buildserver/upload.php index ef9fd306a..7d2badfb0 100755 --- a/support/buildserver/upload.php +++ b/support/buildserver/upload.php @@ -1,57 +1,57 @@ - +