From eb771ef51f917309086eba1a0a983fb6c5154de2 Mon Sep 17 00:00:00 2001 From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com> Date: Sat, 15 Feb 2020 11:01:35 -0700 Subject: [PATCH] 225 adding sharpness parameter, needs emu 0.2.32 or higher to work :) --- src/board_comm/board_comm.c | 6 +++--- src/filter/filter.c | 22 +++++++++++----------- src/filter/filter.h | 2 +- src/filter/kalman.c | 8 +++----- src/filter/kalman.h | 2 +- src/version.h | 2 +- 6 files changed, 20 insertions(+), 22 deletions(-) diff --git a/src/board_comm/board_comm.c b/src/board_comm/board_comm.c index 3ece0c48..50ad96ba 100644 --- a/src/board_comm/board_comm.c +++ b/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) diff --git a/src/filter/filter.c b/src/filter/filter.c index 12fbd64c..abcba6ec 100644 --- a/src/filter/filter.c +++ b/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,13 +131,13 @@ 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); @@ -145,9 +145,9 @@ void filter_data(volatile axisData_t *gyroRateData, volatile axisData_t *gyroAcc // 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) diff --git a/src/filter/filter.h b/src/filter/filter.h index 5b2a6457..88886db1 100644 --- a/src/filter/filter.h +++ b/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; diff --git a/src/filter/kalman.c b/src/filter/kalman.c index 1e1d587e..c963f402 100644 --- a/src/filter/kalman.c +++ b/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) { diff --git a/src/filter/kalman.h b/src/filter/kalman.h index cdee0c2f..78c681af 100644 --- a/src/filter/kalman.h +++ b/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 { diff --git a/src/version.h b/src/version.h index f231fb8d..92574409 100644 --- a/src/version.h +++ b/src/version.h @@ -3,4 +3,4 @@ #define HARDWARE_VERSION 101 #define BOOTLOADER_VERSION 101 -#define FIRMWARE_VERSION 224 +#define FIRMWARE_VERSION 225