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

further accuracy improvement for mag calculation.

Browse source 
git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@263 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2013-02-07 04:59:29 +00:00
parent d1d6a5d5ef
commit 4c0fe94072
2 changed files with 2811 additions and 2811 deletions
Download patch file Download diff file Expand all files Collapse all files

8
src/imu.c
View file

@ -287,6 +287,8 @@ static void getEstimatedAttitude(void)
if (sensors(SENSOR_MAG)) {
#if INACCURATE
heading = _atan2f(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z);
heading = heading + magneticDeclination;
heading = heading / 10;
#else
float rollRAD = (float)angle[ROLL] * RADX10;
float pitchRAD = -(float)angle[PITCH] * RADX10;
@ -299,11 +301,9 @@ static void getEstimatedAttitude(void)
float sp = sinf(pitchRAD);
float Xh = magX * cp + magY * sr * sp + magZ * cr * sp;
float Yh = magY * cr - magZ * sr;
heading = _atan2f(-Yh, Xh); // magnetic heading * 10
float hd = (atan2f(-Yh, Xh) * 1800.0f / M_PI + magneticDeclination) / 10.0f;
heading = hd;
#endif
heading = heading + magneticDeclination;
heading = heading / 10;
if (heading > 180)
heading = heading - 360;
else if (heading < -180)