rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

A possible fix to IMU level-off issue

This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-11-06 14:06:44 +10:00
parent 74dc4face9
commit 14b4c4a1e4
2 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/config/config.c
View File

@ -395,7 +395,7 @@ static void resetConf(void)
// global settings // global settings
masterConfig.current_profile_index = 0; // default profile masterConfig.current_profile_index = 0; // default profile
masterConfig.dcm_kp = 10000; // 1.0 * 10000 masterConfig.dcm_kp = 10000; // 1.0 * 10000
masterConfig.dcm_ki = 30; // 0.003 * 10000 masterConfig.dcm_ki = 0; // 0.003 * 10000
resetAccelerometerTrims(&masterConfig.accZero); resetAccelerometerTrims(&masterConfig.accZero);

24
src/main/flight/imu.c
View File

@ -342,13 +342,13 @@ static bool imuIsAccelerometerHealthy(void)
int32_t accMagnitude = 0; int32_t accMagnitude = 0;
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
accMagnitude += (int32_t)accADC[axis] * accADC[axis]; accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis];
} }
accMagnitude = accMagnitude * 100 / ((int32_t)acc_1G * acc_1G); accMagnitude = accMagnitude * 100 / ((int32_t)acc_1G * acc_1G);
// Accept accel readings only in range 0.85g - 1.15g // Accept accel readings only in range 0.90g - 1.10g
return (72 < accMagnitude) && (accMagnitude < 133); return (81 < accMagnitude) && (accMagnitude < 121);
} }
static bool isMagnetometerHealthy(void) static bool isMagnetometerHealthy(void)
@ -370,17 +370,17 @@ static void imuCalculateEstimatedAttitude(void)
uint32_t deltaT = currentTime - previousIMUUpdateTime; uint32_t deltaT = currentTime - previousIMUUpdateTime;
previousIMUUpdateTime = currentTime; previousIMUUpdateTime = currentTime;
// If reading is considered valid - apply filter
for (axis = 0; axis < 3; axis++) {
if (imuRuntimeConfig->acc_cut_hz > 0) {
accSmooth[axis] = filterApplyPt1(accADC[axis], &accLPFState[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6);
} else {
accSmooth[axis] = accADC[axis];
}
}
// Smooth and use only valid accelerometer readings // Smooth and use only valid accelerometer readings
if (imuIsAccelerometerHealthy()) { if (imuIsAccelerometerHealthy()) {
// If reading is considered valid - apply filter
for (axis = 0; axis < 3; axis++) {
if (imuRuntimeConfig->acc_cut_hz > 0) {
accSmooth[axis] = filterApplyPt1(accADC[axis], &accLPFState[axis], imuRuntimeConfig->acc_cut_hz, deltaT * 1e-6);
} else {
accSmooth[axis] = accADC[axis];
}
}
useAcc = true; useAcc = true;
} }