Skip to content

Commit

Permalink
Imuf230 (#5)
Browse files Browse the repository at this point in the history 
* kalmanCovar without crossInfluence

* Update README.md

* Revert "Merge branch 'master' into IMUF230"

This reverts commit 06a87f4, reversing
changes made to f34b081.

* Clean up
  • Loading branch information
@AndreySemjonov
AndreySemjonov authored May 18, 2020
1 parent 868b59a commit acc7392
Show file tree
Hide file tree
Showing 8 changed files with 64 additions and 95 deletions.
19 changes: 0 additions & 19 deletions .github/workflows/build.yml
View file

This file was deleted.

2 changes: 2 additions & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,3 +13,5 @@ sudo mv ~/Downloads/gcc_arm-6-2017-q1 /usr/local/gcc_arm-6-2017-q1
sudo ln -s /usr/local/gcc_arm-6-2017-q1 /usr/local/gcc_arm
echo 'export PATH="$PATH:/usr/local/gcc_arm/bin"' >> ~/.bash_profile
```
After getting everything that you need to compile, run this command to compile the IMUF
python make.py -T F3
8 changes: 4 additions & 4 deletions src/board_comm/board_comm.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,10 +151,10 @@ static void run_command(volatile imufCommand_t* command, volatile imufCommand_t*
filterConfig.i_roll_lpf_hz = (int16_t)(command->param4 & 0xFFFF);
filterConfig.i_pitch_lpf_hz = (int16_t)(command->param5 >> 16);
filterConfig.i_yaw_lpf_hz = (int16_t)(command->param5 & 0xFFFF);
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.sharpness = (int16_t)(command->param10 >> 16);
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.sharpness = (int16_t) ((int16_t)(command->param10 >> 16));
if (!filterConfig.sharpness)
{
filterConfig.sharpness = 35;
Expand Down
18 changes: 9 additions & 9 deletions src/filter/biquad.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,9 +23,9 @@ void biquad_init(float filterCutFreq, biquad_axis_state_t *state, float refreshR
switch (filterType)
{
case FILTER_TYPE_LOWPASS:
b0 = (1.0f - cs) * 0.5f;
b0 = (1.0f - cs) *0.5f;
b1 = 1.0f - cs;
b2 = (1.0f - cs) * 0.5f;
b2 = (1.0f - cs) *0.5f;
a0 = 1.0f + alpha;
a1 = -2.0f * cs;
a2 = 1.0f - alpha;
Expand All @@ -41,12 +41,12 @@ void biquad_init(float filterCutFreq, biquad_axis_state_t *state, float refreshR
}
//don't let these states be used until they're updated
__disable_irq();
const float a0r = 1.0f / a0;
state->a0 = b0 * a0r;
state->a1 = b1 * a0r;
state->a2 = b2 * a0r;
state->a3 = a1 * a0r;
state->a4 = a2 * a0r;
const float a0r = 1.0f / a0;
state->a0 = b0 * a0r;
state->a1 = b1 * a0r;
state->a2 = b2 * a0r;
state->a3 = a1 * a0r;
state->a4 = a2 * a0r;
__enable_irq();
}

Expand Down Expand Up @@ -78,4 +78,4 @@ float biquad_update(float sample, biquad_axis_state_t *state)
return result;

}
#pragma GCC pop_options
#pragma GCC pop_options
45 changes: 23 additions & 22 deletions src/filter/filter.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,25 +6,25 @@

volatile filter_config_t filterConfig =
{
DEFAULT_ROLL_Q,
DEFAULT_PITCH_Q,
DEFAULT_YAW_Q,
MIN_WINDOW_SIZE,
DEFAULT_ROLL_Q,
DEFAULT_PITCH_Q,
DEFAULT_YAW_Q,
MIN_WINDOW_SIZE,

(float)DEFAULT_ROLL_Q,
(float)DEFAULT_PITCH_Q,
(float)DEFAULT_YAW_Q,
(float)DEFAULT_ROLL_Q,
(float)DEFAULT_PITCH_Q,
(float)DEFAULT_YAW_Q,

(float)BASE_LPF_HZ,
(float)BASE_LPF_HZ,
(float)BASE_LPF_HZ,
(float)BASE_LPF_HZ,
(float)BASE_LPF_HZ,
(float)BASE_LPF_HZ,

40.0f,
40.0f,

BASE_LPF_HZ,
BASE_LPF_HZ,
BASE_LPF_HZ,
100,
BASE_LPF_HZ,
BASE_LPF_HZ,
BASE_LPF_HZ,
100,
};

// PT1 Low Pass filter
Expand Down Expand Up @@ -80,7 +80,7 @@ void filter_init(void)
pt1FilterInit(&ay_filter, k, 0.0f);
pt1FilterInit(&az_filter, k, 0.0f);

sharpness = filterConfig.sharpness / 250.0f;
sharpness = (float)filterConfig.sharpness / 250.0f;
}

void filter_data(volatile axisData_t *gyroRateData, volatile axisData_t *gyroAccData, float gyroTempData, filteredData_t *filteredData)
Expand Down Expand Up @@ -112,27 +112,28 @@ void filter_data(volatile axisData_t *gyroRateData, volatile axisData_t *gyroAcc
float errorMultiplierZ = ABS(setPoint.z - filteredData->rateData.z) * sharpness;

// give a boost to the setpoint, used to caluclate the filter cutoff, based on the error and setpoint/gyrodata
errorMultiplierX = CONSTRAIN(errorMultiplierX * ABS(1.0f - (setPoint.x / filteredData->rateData.x)) + 1.0f, 1.0f, 50.0f);
errorMultiplierY = CONSTRAIN(errorMultiplierY * ABS(1.0f - (setPoint.y / filteredData->rateData.y)) + 1.0f, 1.0f, 50.0f);
errorMultiplierZ = CONSTRAIN(errorMultiplierZ * ABS(1.0f - (setPoint.z / filteredData->rateData.z)) + 1.0f, 1.0f, 50.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)
{
setPointNew = 0;
if (setPoint.x != 0.0f && oldSetPoint.x != setPoint.x)
{
filterConfig.roll_lpf_hz = CONSTRAIN(10.0f * ABS(1.0f - ((setPoint.x * errorMultiplierX) / filteredData->rateData.x)), 10.0f, 500.0f);
filterConfig.roll_lpf_hz = CONSTRAIN((float)filterConfig.i_roll_lpf_hz * ABS(1.0f - ((setPoint.x * errorMultiplierX) / filteredData->rateData.x)), 10.0f, 500.0f);
filter_biquad_init(filterConfig.roll_lpf_hz, &(lpfFilterStateRate.x));
}
if (setPoint.y != 0.0f && oldSetPoint.y != setPoint.y)
{
filterConfig.pitch_lpf_hz = CONSTRAIN(10.0f * ABS(1.0f - ((setPoint.y * errorMultiplierY) / filteredData->rateData.y)), 10.0f, 500.0f);
filterConfig.pitch_lpf_hz = CONSTRAIN((float)filterConfig.i_pitch_lpf_hz * ABS(1.0f - ((setPoint.y * errorMultiplierY) / filteredData->rateData.y)), 10.0f, 500.0f);
filter_biquad_init(filterConfig.pitch_lpf_hz, &(lpfFilterStateRate.y));
}
if (setPoint.z != 0.0f && oldSetPoint.z != setPoint.z)
{
filterConfig.yaw_lpf_hz = CONSTRAIN(10.0f * ABS(1.0f - ((setPoint.z * errorMultiplierZ) / filteredData->rateData.z)), 10.0f, 500.0f);
filterConfig.yaw_lpf_hz = CONSTRAIN((float)filterConfig.i_yaw_lpf_hz * ABS(1.0f - ((setPoint.z * errorMultiplierZ) / filteredData->rateData.z)), 10.0f, 500.0f);
filter_biquad_init(filterConfig.yaw_lpf_hz, &(lpfFilterStateRate.z));
}
memcpy((uint32_t *)&oldSetPoint, (uint32_t *)&setPoint, sizeof(axisData_t));
Expand Down
59 changes: 25 additions & 34 deletions src/filter/kalman.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,51 +29,42 @@ void kalman_init(void)
#pragma GCC optimize("O3")
void update_kalman_covariance(volatile axisData_t *gyroRateData)
{
varStruct.xWindow[ varStruct.windex] = gyroRateData->x;
varStruct.yWindow[ varStruct.windex] = gyroRateData->y;
varStruct.zWindow[ varStruct.windex] = gyroRateData->z;
varStruct.xWindow[ varStruct.windex] = gyroRateData->x;
varStruct.yWindow[ varStruct.windex] = gyroRateData->y;
varStruct.zWindow[ varStruct.windex] = gyroRateData->z;

varStruct.xSumMean += varStruct.xWindow[ varStruct.windex];
varStruct.ySumMean += varStruct.yWindow[ varStruct.windex];
varStruct.zSumMean += varStruct.zWindow[ varStruct.windex];
varStruct.xSumVar = varStruct.xSumVar + ( varStruct.xWindow[ varStruct.windex] * varStruct.xWindow[ varStruct.windex]);
varStruct.ySumVar = varStruct.ySumVar + ( varStruct.yWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]);
varStruct.zSumVar = varStruct.zSumVar + ( varStruct.zWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
varStruct.xySumCoVar = varStruct.xySumCoVar + ( varStruct.xWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]);
varStruct.xzSumCoVar = varStruct.xzSumCoVar + ( varStruct.xWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
varStruct.yzSumCoVar = varStruct.yzSumCoVar + ( varStruct.yWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
varStruct.windex++;
varStruct.xSumMean += varStruct.xWindow[ varStruct.windex];
varStruct.ySumMean += varStruct.yWindow[ varStruct.windex];
varStruct.zSumMean += varStruct.zWindow[ varStruct.windex];
varStruct.xSumVar = varStruct.xSumVar + ( varStruct.xWindow[ varStruct.windex] * varStruct.xWindow[ varStruct.windex]);
varStruct.ySumVar = varStruct.ySumVar + ( varStruct.yWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]);
varStruct.zSumVar = varStruct.zSumVar + ( varStruct.zWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
varStruct.windex++;
if ( varStruct.windex >= filterConfig.w)
{
varStruct.windex = 0;
}
varStruct.xSumMean -= varStruct.xWindow[ varStruct.windex];
varStruct.ySumMean -= varStruct.yWindow[ varStruct.windex];
varStruct.zSumMean -= varStruct.zWindow[ varStruct.windex];
varStruct.xSumVar = varStruct.xSumVar - ( varStruct.xWindow[ varStruct.windex] * varStruct.xWindow[ varStruct.windex]);
varStruct.ySumVar = varStruct.ySumVar - ( varStruct.yWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]);
varStruct.zSumVar = varStruct.zSumVar - ( varStruct.zWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
varStruct.xySumCoVar = varStruct.xySumCoVar - ( varStruct.xWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]);
varStruct.xzSumCoVar = varStruct.xzSumCoVar - ( varStruct.xWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
varStruct.yzSumCoVar = varStruct.yzSumCoVar - ( varStruct.yWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
varStruct.xSumMean -= varStruct.xWindow[ varStruct.windex];
varStruct.ySumMean -= varStruct.yWindow[ varStruct.windex];
varStruct.zSumMean -= varStruct.zWindow[ varStruct.windex];
varStruct.xSumVar = varStruct.xSumVar - ( varStruct.xWindow[ varStruct.windex] * varStruct.xWindow[ varStruct.windex]);
varStruct.ySumVar = varStruct.ySumVar - ( varStruct.yWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]);
varStruct.zSumVar = varStruct.zSumVar - ( varStruct.zWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);

varStruct.xMean = varStruct.xSumMean * varStruct.inverseN;
varStruct.yMean = varStruct.ySumMean * varStruct.inverseN;
varStruct.zMean = varStruct.zSumMean * varStruct.inverseN;
varStruct.xMean = varStruct.xSumMean * varStruct.inverseN;
varStruct.yMean = varStruct.ySumMean * varStruct.inverseN;
varStruct.zMean = varStruct.zSumMean * varStruct.inverseN;

varStruct.xVar = ABS(varStruct.xSumVar * varStruct.inverseN - ( varStruct.xMean * varStruct.xMean));
varStruct.yVar = ABS(varStruct.ySumVar * varStruct.inverseN - ( varStruct.yMean * varStruct.yMean));
varStruct.zVar = ABS(varStruct.zSumVar * varStruct.inverseN - ( varStruct.zMean * varStruct.zMean));
varStruct.xyCoVar = ABS(varStruct.xySumCoVar * varStruct.inverseN - ( varStruct.xMean * varStruct.yMean));
varStruct.xzCoVar = ABS(varStruct.xzSumCoVar * varStruct.inverseN - ( varStruct.xMean * varStruct.zMean));
varStruct.yzCoVar = ABS(varStruct.yzSumCoVar * varStruct.inverseN - ( varStruct.yMean * varStruct.zMean));
varStruct.xVar = ABS(varStruct.xSumVar * varStruct.inverseN - ( varStruct.xMean * varStruct.xMean));
varStruct.yVar = ABS(varStruct.ySumVar * varStruct.inverseN - ( varStruct.yMean * varStruct.yMean));
varStruct.zVar = ABS(varStruct.zSumVar * varStruct.inverseN - ( varStruct.zMean * varStruct.zMean));

float squirt;
arm_sqrt_f32(varStruct.xVar + varStruct.xyCoVar + varStruct.xzCoVar, &squirt);
arm_sqrt_f32(varStruct.xVar, &squirt);
kalmanFilterStateRate[ROLL].r = squirt * VARIANCE_SCALE;
arm_sqrt_f32(varStruct.yVar + varStruct.xyCoVar + varStruct.yzCoVar, &squirt);
arm_sqrt_f32(varStruct.yVar, &squirt);
kalmanFilterStateRate[PITCH].r = squirt * VARIANCE_SCALE;
arm_sqrt_f32(varStruct.zVar + varStruct.yzCoVar + varStruct.xzCoVar, &squirt);
arm_sqrt_f32(varStruct.zVar, &squirt);
kalmanFilterStateRate[YAW].r = squirt * VARIANCE_SCALE;
}

Expand Down
6 changes: 0 additions & 6 deletions src/filter/kalman.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,9 +26,6 @@ typedef struct variance
float xVar;
float yVar;
float zVar;
float xyCoVar;
float xzCoVar;
float yzCoVar;

uint32_t windex;
float xWindow[MAX_WINDOW_SIZE];
Expand All @@ -46,9 +43,6 @@ typedef struct variance
float xSumVar;
float ySumVar;
float zSumVar;
float xySumCoVar;
float xzSumCoVar;
float yzSumCoVar;

float inverseN;
} variance_t;
Expand Down
2 changes: 1 addition & 1 deletion src/version.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,4 +3,4 @@
#define HARDWARE_VERSION 101
#define BOOTLOADER_VERSION 101

#define FIRMWARE_VERSION 227
#define FIRMWARE_VERSION 230

0 comments on commit acc7392

Please sign in to comment.