Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00
Code Issues Wiki Activity

Revert "Removed the hard accelerometer cutoff, replaced my a smooth weighting."

Browse source 
This reverts commit 587ab7178a.
This commit is contained in:
Zap Andersson 2016-03-19 16:58:28 +01:00
parent c0e01e8d2b
commit 451060a8ab
1 changed files with 18 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

37
src/main/flight/imu.c
View file

@ -224,7 +224,7 @@ static float imuGetPGainScaleFactor(void)
} }
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
float useAcc, float ax, float ay, float az, bool useAcc, float ax, float ay, float az,
bool useMag, float mx, float my, float mz, bool useMag, float mx, float my, float mz,
bool useYaw, float yawError) bool useYaw, float yawError)
{ {
@ -272,21 +272,19 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
ez += rMat[2][2] * ez_ef; ez += rMat[2][2] * ez_ef;
} }
if (useAcc > 0.0f) { // Use measured acceleration vector
// Just scale by 1G length - That's our vector adjustment. Rather than recipNorm = sq(ax) + sq(ay) + sq(az);
// using one-over-exact length (which needs a costly square root), we already if (useAcc && recipNorm > 0.01f) {
// know the vector is enough "roughly unit length" and since it is only weighted // Normalise accelerometer measurement
// in by a certain amount anyway later, having that exact is meaningless. recipNorm = invSqrt(recipNorm);
recipNorm = 1.0f / acc_1G;;
ax *= recipNorm; ax *= recipNorm;
ay *= recipNorm; ay *= recipNorm;
az *= recipNorm; az *= recipNorm;
ex += (ay * rMat[2][2] - az * rMat[2][1]) * useAcc; // Error is sum of cross product between estimated direction and measured direction of gravity
ey += (az * rMat[2][0] - ax * rMat[2][2]) * useAcc; ex += (ay * rMat[2][2] - az * rMat[2][1]);
ez += (ax * rMat[2][1] - ay * rMat[2][0]) * useAcc; ey += (az * rMat[2][0] - ax * rMat[2][2]);
ez += (ax * rMat[2][1] - ay * rMat[2][0]);
} }
// Compute and apply integral feedback if enabled // Compute and apply integral feedback if enabled
@ -355,7 +353,7 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
} }
} }
static float imuAccelerometerWeight(void) static bool imuIsAccelerometerHealthy(void)
{ {
int32_t axis; int32_t axis;
int32_t accMagnitude = 0; int32_t accMagnitude = 0;
@ -364,11 +362,10 @@ static float imuAccelerometerWeight(void)
accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis]; accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis];
} }
accMagnitude = accMagnitude * 100 / ((((int32_t)acc_1G)*((int32_t)acc_1G))); accMagnitude = accMagnitude * 100 / (sq((int32_t)acc_1G));
int32_t nearness = ABS(100 - accMagnitude); // Accept accel readings only in range 0.90g - 1.10g
if (nearness > 25) return 0.0f; return (81 < accMagnitude) && (accMagnitude < 121);
return 1.0f - (nearness / 25.0f);
} }
static bool isMagnetometerHealthy(void) static bool isMagnetometerHealthy(void)
@ -380,7 +377,7 @@ static void imuCalculateEstimatedAttitude(void)
{ {
static uint32_t previousIMUUpdateTime; static uint32_t previousIMUUpdateTime;
float rawYawError = 0; float rawYawError = 0;
float useAcc = 0.0f; bool useAcc = false;
bool useMag = false; bool useMag = false;
bool useYaw = false; bool useYaw = false;
@ -388,7 +385,9 @@ static void imuCalculateEstimatedAttitude(void)
uint32_t deltaT = currentTime - previousIMUUpdateTime; uint32_t deltaT = currentTime - previousIMUUpdateTime;
previousIMUUpdateTime = currentTime; previousIMUUpdateTime = currentTime;
useAcc = imuAccelerometerWeight(); if (imuIsAccelerometerHealthy()) {
useAcc = true;
}
if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) { if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) {
useMag = true; useMag = true;