Skip to content

Commit acc7392

Browse files
AndreySemjonovAndreySemjonov
authored
Imuf230 (#5)
* kalmanCovar without crossInfluence * Update README.md * Revert "Merge branch 'master' into IMUF230" This reverts commit 06a87f4, reversing changes made to f34b081. * Clean up
1 parent 868b59a commit acc7392

File tree

8 files changed

+64
-95
lines changed

8 files changed

+64
-95
lines changed

‎.github/workflows/build.yml‎

Lines changed: 0 additions & 19 deletions
This file was deleted.

‎README.md‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -13,3 +13,5 @@ sudo mv ~/Downloads/gcc_arm-6-2017-q1 /usr/local/gcc_arm-6-2017-q1
1313
sudo ln -s /usr/local/gcc_arm-6-2017-q1 /usr/local/gcc_arm
1414
echo 'export PATH="$PATH:/usr/local/gcc_arm/bin"' >> ~/.bash_profile
1515
```
16+
After getting everything that you need to compile, run this command to compile the IMUF
17+
python make.py -T F3

‎src/board_comm/board_comm.c‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -151,10 +151,10 @@ static void run_command(volatile imufCommand_t* command, volatile imufCommand_t*
151151
filterConfig.i_roll_lpf_hz = (int16_t)(command->param4 & 0xFFFF);
152152
filterConfig.i_pitch_lpf_hz = (int16_t)(command->param5 >> 16);
153153
filterConfig.i_yaw_lpf_hz = (int16_t)(command->param5 & 0xFFFF);
154-
gyroSettingsConfig.smallX = (int32_t)((int16_t)(command->param8 >> 16));
155-
gyroSettingsConfig.smallY = (int32_t)((int16_t)(command->param9 & 0xFFFF));
156-
gyroSettingsConfig.smallZ = (int32_t)((int16_t)(command->param9 >> 16));
157-
filterConfig.sharpness = (int16_t)(command->param10 >> 16);
154+
gyroSettingsConfig.smallX = (int32_t) ((int16_t)(command->param8 >> 16));
155+
gyroSettingsConfig.smallY = (int32_t) ((int16_t)(command->param9 & 0xFFFF));
156+
gyroSettingsConfig.smallZ = (int32_t) ((int16_t)(command->param9 >> 16));
157+
filterConfig.sharpness = (int16_t) ((int16_t)(command->param10 >> 16));
158158
if (!filterConfig.sharpness)
159159
{
160160
filterConfig.sharpness = 35;

‎src/filter/biquad.c‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -23,9 +23,9 @@ void biquad_init(float filterCutFreq, biquad_axis_state_t *state, float refreshR
2323
switch (filterType)
2424
{
2525
case FILTER_TYPE_LOWPASS:
26-
b0 = (1.0f - cs) * 0.5f;
26+
b0 = (1.0f - cs) *0.5f;
2727
b1 = 1.0f - cs;
28-
b2 = (1.0f - cs) * 0.5f;
28+
b2 = (1.0f - cs) *0.5f;
2929
a0 = 1.0f + alpha;
3030
a1 = -2.0f * cs;
3131
a2 = 1.0f - alpha;
@@ -41,12 +41,12 @@ void biquad_init(float filterCutFreq, biquad_axis_state_t *state, float refreshR
4141
}
4242
//don't let these states be used until they're updated
4343
__disable_irq();
44-
const float a0r = 1.0f / a0;
45-
state->a0 = b0 * a0r;
46-
state->a1 = b1 * a0r;
47-
state->a2 = b2 * a0r;
48-
state->a3 = a1 * a0r;
49-
state->a4 = a2 * a0r;
44+
const float a0r = 1.0f / a0;
45+
state->a0 = b0 * a0r;
46+
state->a1 = b1 * a0r;
47+
state->a2 = b2 * a0r;
48+
state->a3 = a1 * a0r;
49+
state->a4 = a2 * a0r;
5050
__enable_irq();
5151
}
5252

@@ -78,4 +78,4 @@ float biquad_update(float sample, biquad_axis_state_t *state)
7878
return result;
7979

8080
}
81-
#pragma GCC pop_options
81+
#pragma GCC pop_options

‎src/filter/filter.c‎

Lines changed: 23 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -6,25 +6,25 @@
66

77
volatile filter_config_t filterConfig =
88
{
9-
DEFAULT_ROLL_Q,
10-
DEFAULT_PITCH_Q,
11-
DEFAULT_YAW_Q,
12-
MIN_WINDOW_SIZE,
9+
DEFAULT_ROLL_Q,
10+
DEFAULT_PITCH_Q,
11+
DEFAULT_YAW_Q,
12+
MIN_WINDOW_SIZE,
1313

14-
(float)DEFAULT_ROLL_Q,
15-
(float)DEFAULT_PITCH_Q,
16-
(float)DEFAULT_YAW_Q,
14+
(float)DEFAULT_ROLL_Q,
15+
(float)DEFAULT_PITCH_Q,
16+
(float)DEFAULT_YAW_Q,
1717

18-
(float)BASE_LPF_HZ,
19-
(float)BASE_LPF_HZ,
20-
(float)BASE_LPF_HZ,
18+
(float)BASE_LPF_HZ,
19+
(float)BASE_LPF_HZ,
20+
(float)BASE_LPF_HZ,
2121

22-
40.0f,
22+
40.0f,
2323

24-
BASE_LPF_HZ,
25-
BASE_LPF_HZ,
26-
BASE_LPF_HZ,
27-
100,
24+
BASE_LPF_HZ,
25+
BASE_LPF_HZ,
26+
BASE_LPF_HZ,
27+
100,
2828
};
2929

3030
// PT1 Low Pass filter
@@ -80,7 +80,7 @@ void filter_init(void)
8080
pt1FilterInit(&ay_filter, k, 0.0f);
8181
pt1FilterInit(&az_filter, k, 0.0f);
8282

83-
sharpness = filterConfig.sharpness / 250.0f;
83+
sharpness = (float)filterConfig.sharpness / 250.0f;
8484
}
8585

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

114114
// give a boost to the setpoint, used to caluclate the filter cutoff, based on the error and setpoint/gyrodata
115-
errorMultiplierX = CONSTRAIN(errorMultiplierX * ABS(1.0f - (setPoint.x / filteredData->rateData.x)) + 1.0f, 1.0f, 50.0f);
116-
errorMultiplierY = CONSTRAIN(errorMultiplierY * ABS(1.0f - (setPoint.y / filteredData->rateData.y)) + 1.0f, 1.0f, 50.0f);
117-
errorMultiplierZ = CONSTRAIN(errorMultiplierZ * ABS(1.0f - (setPoint.z / filteredData->rateData.z)) + 1.0f, 1.0f, 50.0f);
115+
116+
errorMultiplierX = CONSTRAIN(errorMultiplierX * ABS(1.0f - (setPoint.x / filteredData->rateData.x)) + 1.0f, 1.0f, 10.0f);
117+
errorMultiplierY = CONSTRAIN(errorMultiplierY * ABS(1.0f - (setPoint.y / filteredData->rateData.y)) + 1.0f, 1.0f, 10.0f);
118+
errorMultiplierZ = CONSTRAIN(errorMultiplierZ * ABS(1.0f - (setPoint.z / filteredData->rateData.z)) + 1.0f, 1.0f, 10.0f);
118119

119120

120121
if (setPointNew)
121122
{
122123
setPointNew = 0;
123124
if (setPoint.x != 0.0f && oldSetPoint.x != setPoint.x)
124125
{
125-
filterConfig.roll_lpf_hz = CONSTRAIN(10.0f * ABS(1.0f - ((setPoint.x * errorMultiplierX) / filteredData->rateData.x)), 10.0f, 500.0f);
126+
filterConfig.roll_lpf_hz = CONSTRAIN((float)filterConfig.i_roll_lpf_hz * ABS(1.0f - ((setPoint.x * errorMultiplierX) / filteredData->rateData.x)), 10.0f, 500.0f);
126127
filter_biquad_init(filterConfig.roll_lpf_hz, &(lpfFilterStateRate.x));
127128
}
128129
if (setPoint.y != 0.0f && oldSetPoint.y != setPoint.y)
129130
{
130-
filterConfig.pitch_lpf_hz = CONSTRAIN(10.0f * ABS(1.0f - ((setPoint.y * errorMultiplierY) / filteredData->rateData.y)), 10.0f, 500.0f);
131+
filterConfig.pitch_lpf_hz = CONSTRAIN((float)filterConfig.i_pitch_lpf_hz * ABS(1.0f - ((setPoint.y * errorMultiplierY) / filteredData->rateData.y)), 10.0f, 500.0f);
131132
filter_biquad_init(filterConfig.pitch_lpf_hz, &(lpfFilterStateRate.y));
132133
}
133134
if (setPoint.z != 0.0f && oldSetPoint.z != setPoint.z)
134135
{
135-
filterConfig.yaw_lpf_hz = CONSTRAIN(10.0f * ABS(1.0f - ((setPoint.z * errorMultiplierZ) / filteredData->rateData.z)), 10.0f, 500.0f);
136+
filterConfig.yaw_lpf_hz = CONSTRAIN((float)filterConfig.i_yaw_lpf_hz * ABS(1.0f - ((setPoint.z * errorMultiplierZ) / filteredData->rateData.z)), 10.0f, 500.0f);
136137
filter_biquad_init(filterConfig.yaw_lpf_hz, &(lpfFilterStateRate.z));
137138
}
138139
memcpy((uint32_t *)&oldSetPoint, (uint32_t *)&setPoint, sizeof(axisData_t));

‎src/filter/kalman.c‎

Lines changed: 25 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -29,51 +29,42 @@ void kalman_init(void)
2929
#pragma GCC optimize("O3")
3030
void update_kalman_covariance(volatile axisData_t *gyroRateData)
3131
{
32-
varStruct.xWindow[ varStruct.windex] = gyroRateData->x;
33-
varStruct.yWindow[ varStruct.windex] = gyroRateData->y;
34-
varStruct.zWindow[ varStruct.windex] = gyroRateData->z;
32+
varStruct.xWindow[ varStruct.windex] = gyroRateData->x;
33+
varStruct.yWindow[ varStruct.windex] = gyroRateData->y;
34+
varStruct.zWindow[ varStruct.windex] = gyroRateData->z;
3535

36-
varStruct.xSumMean += varStruct.xWindow[ varStruct.windex];
37-
varStruct.ySumMean += varStruct.yWindow[ varStruct.windex];
38-
varStruct.zSumMean += varStruct.zWindow[ varStruct.windex];
39-
varStruct.xSumVar = varStruct.xSumVar + ( varStruct.xWindow[ varStruct.windex] * varStruct.xWindow[ varStruct.windex]);
40-
varStruct.ySumVar = varStruct.ySumVar + ( varStruct.yWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]);
41-
varStruct.zSumVar = varStruct.zSumVar + ( varStruct.zWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
42-
varStruct.xySumCoVar = varStruct.xySumCoVar + ( varStruct.xWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]);
43-
varStruct.xzSumCoVar = varStruct.xzSumCoVar + ( varStruct.xWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
44-
varStruct.yzSumCoVar = varStruct.yzSumCoVar + ( varStruct.yWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
45-
varStruct.windex++;
36+
varStruct.xSumMean += varStruct.xWindow[ varStruct.windex];
37+
varStruct.ySumMean += varStruct.yWindow[ varStruct.windex];
38+
varStruct.zSumMean += varStruct.zWindow[ varStruct.windex];
39+
varStruct.xSumVar = varStruct.xSumVar + ( varStruct.xWindow[ varStruct.windex] * varStruct.xWindow[ varStruct.windex]);
40+
varStruct.ySumVar = varStruct.ySumVar + ( varStruct.yWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]);
41+
varStruct.zSumVar = varStruct.zSumVar + ( varStruct.zWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
42+
varStruct.windex++;
4643
if ( varStruct.windex >= filterConfig.w)
4744
{
4845
varStruct.windex = 0;
4946
}
50-
varStruct.xSumMean -= varStruct.xWindow[ varStruct.windex];
51-
varStruct.ySumMean -= varStruct.yWindow[ varStruct.windex];
52-
varStruct.zSumMean -= varStruct.zWindow[ varStruct.windex];
53-
varStruct.xSumVar = varStruct.xSumVar - ( varStruct.xWindow[ varStruct.windex] * varStruct.xWindow[ varStruct.windex]);
54-
varStruct.ySumVar = varStruct.ySumVar - ( varStruct.yWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]);
55-
varStruct.zSumVar = varStruct.zSumVar - ( varStruct.zWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
56-
varStruct.xySumCoVar = varStruct.xySumCoVar - ( varStruct.xWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]);
57-
varStruct.xzSumCoVar = varStruct.xzSumCoVar - ( varStruct.xWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
58-
varStruct.yzSumCoVar = varStruct.yzSumCoVar - ( varStruct.yWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
47+
varStruct.xSumMean -= varStruct.xWindow[ varStruct.windex];
48+
varStruct.ySumMean -= varStruct.yWindow[ varStruct.windex];
49+
varStruct.zSumMean -= varStruct.zWindow[ varStruct.windex];
50+
varStruct.xSumVar = varStruct.xSumVar - ( varStruct.xWindow[ varStruct.windex] * varStruct.xWindow[ varStruct.windex]);
51+
varStruct.ySumVar = varStruct.ySumVar - ( varStruct.yWindow[ varStruct.windex] * varStruct.yWindow[ varStruct.windex]);
52+
varStruct.zSumVar = varStruct.zSumVar - ( varStruct.zWindow[ varStruct.windex] * varStruct.zWindow[ varStruct.windex]);
5953

60-
varStruct.xMean = varStruct.xSumMean * varStruct.inverseN;
61-
varStruct.yMean = varStruct.ySumMean * varStruct.inverseN;
62-
varStruct.zMean = varStruct.zSumMean * varStruct.inverseN;
54+
varStruct.xMean = varStruct.xSumMean * varStruct.inverseN;
55+
varStruct.yMean = varStruct.ySumMean * varStruct.inverseN;
56+
varStruct.zMean = varStruct.zSumMean * varStruct.inverseN;
6357

64-
varStruct.xVar = ABS(varStruct.xSumVar * varStruct.inverseN - ( varStruct.xMean * varStruct.xMean));
65-
varStruct.yVar = ABS(varStruct.ySumVar * varStruct.inverseN - ( varStruct.yMean * varStruct.yMean));
66-
varStruct.zVar = ABS(varStruct.zSumVar * varStruct.inverseN - ( varStruct.zMean * varStruct.zMean));
67-
varStruct.xyCoVar = ABS(varStruct.xySumCoVar * varStruct.inverseN - ( varStruct.xMean * varStruct.yMean));
68-
varStruct.xzCoVar = ABS(varStruct.xzSumCoVar * varStruct.inverseN - ( varStruct.xMean * varStruct.zMean));
69-
varStruct.yzCoVar = ABS(varStruct.yzSumCoVar * varStruct.inverseN - ( varStruct.yMean * varStruct.zMean));
58+
varStruct.xVar = ABS(varStruct.xSumVar * varStruct.inverseN - ( varStruct.xMean * varStruct.xMean));
59+
varStruct.yVar = ABS(varStruct.ySumVar * varStruct.inverseN - ( varStruct.yMean * varStruct.yMean));
60+
varStruct.zVar = ABS(varStruct.zSumVar * varStruct.inverseN - ( varStruct.zMean * varStruct.zMean));
7061

7162
float squirt;
72-
arm_sqrt_f32(varStruct.xVar + varStruct.xyCoVar + varStruct.xzCoVar, &squirt);
63+
arm_sqrt_f32(varStruct.xVar, &squirt);
7364
kalmanFilterStateRate[ROLL].r = squirt * VARIANCE_SCALE;
74-
arm_sqrt_f32(varStruct.yVar + varStruct.xyCoVar + varStruct.yzCoVar, &squirt);
65+
arm_sqrt_f32(varStruct.yVar, &squirt);
7566
kalmanFilterStateRate[PITCH].r = squirt * VARIANCE_SCALE;
76-
arm_sqrt_f32(varStruct.zVar + varStruct.yzCoVar + varStruct.xzCoVar, &squirt);
67+
arm_sqrt_f32(varStruct.zVar, &squirt);
7768
kalmanFilterStateRate[YAW].r = squirt * VARIANCE_SCALE;
7869
}
7970

‎src/filter/kalman.h‎

Lines changed: 0 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -26,9 +26,6 @@ typedef struct variance
2626
float xVar;
2727
float yVar;
2828
float zVar;
29-
float xyCoVar;
30-
float xzCoVar;
31-
float yzCoVar;
3229

3330
uint32_t windex;
3431
float xWindow[MAX_WINDOW_SIZE];
@@ -46,9 +43,6 @@ typedef struct variance
4643
float xSumVar;
4744
float ySumVar;
4845
float zSumVar;
49-
float xySumCoVar;
50-
float xzSumCoVar;
51-
float yzSumCoVar;
5246

5347
float inverseN;
5448
} variance_t;

‎src/version.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,4 +3,4 @@
33
#define HARDWARE_VERSION 101
44
#define BOOTLOADER_VERSION 101
55

6-
#define FIRMWARE_VERSION 227
6+
#define FIRMWARE_VERSION 230

0 commit comments

Comments
 (0)