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:
parent
785443789c
commit
d1d6a5d5ef
5924
obj/baseflight.hex
5924
obj/baseflight.hex
File diff suppressed because it is too large
Load Diff
|
@ -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
16
src/imu.c
|
@ -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;
|
||||
|
||||
|
|
Loading…
Reference in New Issue