From c8f0fc78b31b86bc101bddff521cfa37a7d56ad6 Mon Sep 17 00:00:00 2001 From: "timecop@gmail.com" Date: Sun, 22 Sep 2013 04:03:18 +0000 Subject: [PATCH] removed references to avr optimizations we stopped using long time ago fixed heading calculation jump git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@413 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61 --- src/imu.c | 21 ++++++++------------- 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/src/imu.c b/src/imu.c index 6844706354..696c899739 100755 --- a/src/imu.c +++ b/src/imu.c @@ -87,9 +87,6 @@ void computeIMU(void) // // The following ideas was used in this project: // 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix -// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation -// 3) C. Hastings approximation for atan2() -// 4) Optimization tricks: http://www.hackersdelight.org/ // // Currently Magnetometer uses separate CF which is used only // for heading approximation. @@ -277,24 +274,22 @@ static void getEstimatedAttitude(void) // Attitude of the estimated vector anglerad[ROLL] = atan2f(EstG.V.Y, EstG.V.Z); anglerad[PITCH] = atan2f(-EstG.V.X, sqrtf(EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)); - angle[ROLL] = anglerad[ROLL] * (1800.0f / M_PI); // roll angle in 0.1 deg steps - angle[PITCH] = anglerad[PITCH] * (1800.0f / M_PI); // pitch angle in 0.1 deg steps + angle[ROLL] = lrintf(anglerad[ROLL] * (1800.0f / M_PI)); + angle[PITCH] = lrintf(anglerad[PITCH] * (1800.0f / M_PI)); #ifdef MAG if (sensors(SENSOR_MAG)) { - // baseflight calculation by Luggi09 + // baseflight calculation by Luggi09 originates from arducopter float cosineRoll = cosf(anglerad[ROLL]); float sineRoll = sinf(anglerad[ROLL]); float cosinePitch = cosf(anglerad[PITCH]); float sinePitch = sinf(anglerad[PITCH]); - float Xh = EstM.A[X] * cosinePitch + EstM.A[Z] * sinePitch; - float Yh = EstM.A[X] * sinePitch * sineRoll + EstM.A[Y] * cosineRoll - EstM.A[Z] * sineRoll * cosinePitch; + float Xh = EstM.A[X] * cosinePitch + EstM.A[Y] * sineRoll * sinePitch + EstM.A[Z] * sinePitch * cosineRoll; + float Yh = EstM.A[Y] * cosineRoll - EstM.A[Z] * sineRoll; float hd = (atan2f(Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f; - heading = hd; - if (heading > 180) - heading = heading - 360; - else if (heading < -180) - heading = heading + 360; + heading = lrintf(hd); + if (heading < 0) + heading += 360; } #endif