225

adding sharpness parameter, needs emu 0.2.32 or higher to work :)

Author: Quick-Flash

src/board_comm/board_comm.c

@@ -154,10 +154,10 @@ static void run_command(volatile imufCommand_t* command, volatile imufCommand_t*
                 gyroSettingsConfig.smallX        = (int32_t) ((int16_t)(command->param8 >> 16));
                 gyroSettingsConfig.smallY        = (int32_t) ((int16_t)(command->param9 & 0xFFFF));
                 gyroSettingsConfig.smallZ        = (int32_t) ((int16_t)(command->param9 >> 16));
-                filterConfig.r_weight            = (int16_t) ((int16_t)(command->param10 >> 16));
-                if (!filterConfig.r_weight)
+                filterConfig.sharpness            = (int16_t) ((int16_t)(command->param10 >> 16));
+                if (!filterConfig.sharpness)
                 {
-                	filterConfig.r_weight = 100;
+                	filterConfig.sharpness = 35;
                 }
                 filterConfig.acc_lpf_hz          = (int16_t)(command->param10 & 0xFFFF);
                 if (!filterConfig.acc_lpf_hz)

src/filter/filter.c

@@ -73,7 +73,7 @@ volatile axisData_t oldSetPoint;
 volatile axisData_t setPoint;
 volatile int allowFilterInit = 1;
 
-extern float r_filter_weight;
+float sharpness;
 
 void allow_filter_init(void)
 {
@@ -104,7 +104,7 @@ void filter_init(void)
 	pt1FilterInit(&ay_filter, k, 0.0f);
 	pt1FilterInit(&az_filter, k, 0.0f);
 
-	r_filter_weight = (float)filterConfig.r_weight / 100.0f;
+	sharpness = (float)filterConfig.sharpness / 250.0f;
 }
 
 void filter_data(volatile axisData_t *gyroRateData, volatile axisData_t *gyroAccData, float gyroTempData, filteredData_t *filteredData)
@@ -131,23 +131,23 @@ void filter_data(volatile axisData_t *gyroRateData, volatile axisData_t *gyroAcc
 	filteredData->rateData.z = biquad_update(filteredData->rateData.z, &(lpfFilterStateRate.z));
 
 // calculate the error
-	float errorMultiplierX = ABS(setPoint.x - filteredData->rateData.x / 6.0f);
-	float errorMultiplierY = ABS(setPoint.y - filteredData->rateData.y / 6.0f);
-	float errorMultiplierZ = ABS(setPoint.z - filteredData->rateData.z / 6.0f);
+	float errorMultiplierX = ABS(setPoint.x - filteredData->rateData.x) * sharpness;
+	float errorMultiplierY = ABS(setPoint.y - filteredData->rateData.y) * sharpness;
+	float errorMultiplierZ = ABS(setPoint.z - filteredData->rateData.z) * sharpness;
 
-  float setPointChangeBoostX = CONSTRAIN((ABS(setPoint.x - setPoint.x) / 10.0f) + 1.0f, 1.0f, 10.0f);
-	float setPointChangeBoostY = CONSTRAIN((ABS(setPoint.y - setPoint.y) / 10.0f) + 1.0f, 1.0f, 10.0f);
-	float setPointChangeBoostZ = CONSTRAIN((ABS(setPoint.z - setPoint.z) / 10.0f) + 1.0f, 1.0f, 10.0f);
+//  float setPointChangeBoostX = CONSTRAIN((ABS(setPoint.x - setPoint.x) / 10.0f) + 1.0f, 1.0f, 10.0f);
+//  float setPointChangeBoostY = CONSTRAIN((ABS(setPoint.y - setPoint.y) / 10.0f) + 1.0f, 1.0f, 10.0f);
+//  float setPointChangeBoostZ = CONSTRAIN((ABS(setPoint.z - setPoint.z) / 10.0f) + 1.0f, 1.0f, 10.0f);
 
 //	errorMultiplierX = errorMultiplierX * CONSTRAIN(ABS(setPoint.x / 100.0f), 0.1f, 1.0f);
 //	errorMultiplierY = errorMultiplierY * CONSTRAIN(ABS(setPoint.y / 100.0f), 0.1f, 1.0f);
 //	errorMultiplierZ = errorMultiplierZ * CONSTRAIN(ABS(setPoint.z / 100.0f), 0.1f, 1.0f);
 
 // give a boost to the setpoint, used to caluclate the filter cutoff, based on the error and setpoint/gyrodata
 
-	errorMultiplierX = CONSTRAIN(errorMultiplierX * setPointChangeBoostX * ABS(1.0f - (setPoint.x / filteredData->rateData.x)) + 1.0f, 1.0f, 10.0f);
-	errorMultiplierY = CONSTRAIN(errorMultiplierY * setPointChangeBoostX * ABS(1.0f - (setPoint.y / filteredData->rateData.y)) + 1.0f, 1.0f, 10.0f);
-	errorMultiplierZ = CONSTRAIN(errorMultiplierZ * setPointChangeBoostX * ABS(1.0f - (setPoint.z / filteredData->rateData.z)) + 1.0f, 1.0f, 10.0f);
+	errorMultiplierX = CONSTRAIN(errorMultiplierX * ABS(1.0f - (setPoint.x / filteredData->rateData.x)) + 1.0f, 1.0f, 10.0f);
+	errorMultiplierY = CONSTRAIN(errorMultiplierY * ABS(1.0f - (setPoint.y / filteredData->rateData.y)) + 1.0f, 1.0f, 10.0f);
+	errorMultiplierZ = CONSTRAIN(errorMultiplierZ * ABS(1.0f - (setPoint.z / filteredData->rateData.z)) + 1.0f, 1.0f, 10.0f);
 
 
 	if (setPointNew)

src/filter/filter.h

@@ -30,7 +30,7 @@ typedef struct filter_config
     uint16_t i_roll_lpf_hz;
     uint16_t i_pitch_lpf_hz;
     uint16_t i_yaw_lpf_hz;
-    uint16_t r_weight;
+    uint16_t sharpness;
 } filter_config_t;
 
 extern volatile filter_config_t filterConfig;

src/filter/kalman.c

@@ -5,8 +5,6 @@
 
 variance_t varStruct;
 kalman_t   kalmanFilterStateRate[3];
-float      r_filter_weight = 1.0f;
-
 
 void init_kalman(kalman_t *filter, float q)
 {
@@ -72,11 +70,11 @@ void update_kalman_covariance(volatile axisData_t *gyroRateData)
 
     float squirt;
     arm_sqrt_f32(varStruct.xVar +  varStruct.xyCoVar +  varStruct.xzCoVar, &squirt);
-    kalmanFilterStateRate[ROLL].r = squirt * r_filter_weight;
+    kalmanFilterStateRate[ROLL].r = squirt * VARIANCE_SCALE;
     arm_sqrt_f32(varStruct.yVar +  varStruct.xyCoVar +  varStruct.yzCoVar, &squirt);
-    kalmanFilterStateRate[PITCH].r = squirt * r_filter_weight;
+    kalmanFilterStateRate[PITCH].r = squirt * VARIANCE_SCALE;
     arm_sqrt_f32(varStruct.zVar +  varStruct.yzCoVar +  varStruct.xzCoVar, &squirt);
-    kalmanFilterStateRate[YAW].r = squirt * r_filter_weight;
+    kalmanFilterStateRate[YAW].r = squirt * VARIANCE_SCALE;
 }
 
 inline float kalman_process(kalman_t* kalmanState, volatile float input, volatile float target) {

src/filter/kalman.h

@@ -8,7 +8,7 @@
 #define MIN_WINDOW_SIZE 6
 
 // #define VARIANCE_SCALE 0.001
-#define VARIANCE_SCALE 0.3333333f
+#define VARIANCE_SCALE 0.67f
 
 typedef struct kalman
 {

src/version.h

@@ -3,4 +3,4 @@
 #define HARDWARE_VERSION 101
 #define BOOTLOADER_VERSION 101
 
-#define FIRMWARE_VERSION 224
+#define FIRMWARE_VERSION 225