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

Merge pull request #6598 from mikeller/fix_accelerometer_problems 

Fixed problems with accelerometer.
This commit is contained in:
Michael Keller 2018-08-20 16:55:12 +12:00 committed by GitHub
parent 1d63edf7c5 6394cc275d
commit dca0f13692
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -355,16 +355,16 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
static bool imuIsAccelerometerHealthy(float *accAverage) static bool imuIsAccelerometerHealthy(float *accAverage)
{ {
float accMagnitude = 0; float accMagnitudeSq = 0;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
const float a = accAverage[axis]; const float a = accAverage[axis];
accMagnitude += a * a; accMagnitudeSq += a * a;
} }
accMagnitude = sqrtf(accMagnitude) * acc.dev.acc_1G_rec; accMagnitudeSq = accMagnitudeSq * sq(acc.dev.acc_1G_rec);
// Accept accel readings only in range 0.80g - 1.20g // Accept accel readings only in range 0.9g - 1.1g
return (0.80f <= accMagnitude) && (accMagnitude <= 1.20f); return (0.81f < accMagnitudeSq) && (accMagnitudeSq < 1.21f);
} }
// Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling. // Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling.

2
src/main/sensors/acceleration.c
View File

@ -362,7 +362,7 @@ bool accInit(uint32_t gyroSamplingInverval)
} }
acc.dev.acc_1G = 256; // set default acc.dev.acc_1G = 256; // set default
acc.dev.initFn(&acc.dev); // driver initialisation acc.dev.initFn(&acc.dev); // driver initialisation
acc.dev.acc_1G_rec = 1 / acc.dev.acc_1G; acc.dev.acc_1G_rec = 1.0f / acc.dev.acc_1G;
// set the acc sampling interval according to the gyro sampling interval // set the acc sampling interval according to the gyro sampling interval
switch (gyroSamplingInverval) { // Switch statement kept in place to change acc sampling interval in the future switch (gyroSamplingInverval) { // Switch statement kept in place to change acc sampling interval in the future
case 500: case 500: