rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

very nice proper magnetometer vector rotation from crashpilot1000 

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@261 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2013-02-06 04:08:10 +00:00
parent 785443789c
commit d1d6a5d5ef
3 changed files with 2982 additions and 2960 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5924
obj/baseflight.hex
View File

File diff suppressed because it is too large Load Diff

2
src/board.h
View File

@ -19,6 +19,8 @@
#define M_PI 3.14159265358979323846f
#endif /* M_PI */
#define RADX10 (M_PI / 1800.0f) // 0.001745329252f
typedef enum {
SENSOR_ACC = 1 << 0,
SENSOR_BARO = 1 << 1,

16
src/imu.c
View File

@ -285,8 +285,22 @@ static void getEstimatedAttitude(void)
#ifdef MAG
if (sensors(SENSOR_MAG)) {
// Attitude of the cross product vector GxM
#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);
#else
float rollRAD = (float)angle[ROLL] * RADX10;
float pitchRAD = -(float)angle[PITCH] * RADX10;
float magX = EstM.A[1]; // Swap X/Y
float magY = EstM.A[0]; // Swap X/Y
float magZ = EstM.A[2];
float cr = cosf(rollRAD);
float sr = sinf(rollRAD);
float cp = cosf(pitchRAD);
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
#endif
heading = heading + magneticDeclination;
heading = heading / 10;