wip for today.

Quick-Flash · Jun 26, 2019 · 0b8b37f · 0b8b37f
1 parent ca91d45
commit 0b8b37f
Show file tree

Hide file tree

Showing 22 changed files with 263 additions and 153 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -23,7 +23,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 
-#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
@@ -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
+#endif
diff --git 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
@@ -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
@@ -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);