Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00
Code Issues Wiki Activity

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

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