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