I believe ( and I measure) there is a magnetometer problem. The calculation in imu.ino is not fully correct and deals not correctly with the inclination of earthfield.
I have derived a different formula, currently adopted to a HK328 board.
if you need a highly correct heading from the magnetometer-measurements,
you may have problems when tilting the roll or pitch axis.
Code below is with lots of comment
Code: Select all
typedef union { float f; int16_t i;} f32_i16; //to save 12bytes of stack
....
f32_i16 Gx;
f32_i16 Gy;
f32_i16 Gz;
f32_i16 Mx;
f32_i16 My;
f32_i16 Mz;
float fltmp, fl_sqxy;
.......
Gx.f=EstG32.V.X;
Gy.f=EstG32.V.Y;
Gz.f=EstG32.V.Z;
fl_sqxy=Gx.f*Gx.f + Gy.f*Gy.f;
fltmp=InvSqrt(fl_sqxy); // 1/sqrt(Gx^2+Gy^2)
fltmp=fltmp*_flpervatan(fltmp*Gz.f); // 1/sqrt(Gx^2+Gy^2) * atan(Gz/sqrt(Gx^2+Gy^2))
angle[ROLL] = Gx.f*fltmp;
angle[PITCH] = Gy.f*fltmp;
//----- tilt- calculation in G-field completetd !!
invG=InvSqrt(fl_sqxy+Gz.f*Gz.f); // 1/sqrt(Gx^2+Gy^2+Gz^2)
//------ InvG for Barometer and Magnetometer completed
// Start of Magnetometer angular calculations
Mx.f=EstM32.V.X;
My.f=EstM32.V.Y;
Mz.f=EstM32.V.Z;
/*
Zeta= rotates around Z_axis ; tilt = rotates around an axis in x/y Plane
MAG_ABS_COORD(x,y,z)T =(Rotate(-zeta)*Rotate(tilt)*Rotate(zeta)*MagVector)
MAG_ABS_COORD(x)=(Gx*(Gz*(Gy*My+Gx*Mx)-(Gy^2+Gx^2)*Mz)*1/sqrt(Gz^2+Gy^2+Gx^2) - Gy*(Gx*My-Gy*Mx))
MAG_ABS_COORD(y)(Gy*(Gz*(Gy*My+Gx*Mx)-(Gy^2+Gx^2)*Mz)*1/sqrt(Gz^2+Gy^2+Gx^2) + Gx*(Gx*My-Gy*Mx))
MAG_ABS_COORD(z)(Gy^2+Gx^2)*(Gz*Mz + (Gy*My+Gx*Mx))
North is the projection to horizon which is atan2 (MAG_ABS_COORD(x),MAG_ABS_COORD(y))
or maybe 90 - atan2 (MAG_ABS_COORD(y),MAG_ABS_COORD(x))
rotation around Z rotates x/y Magnetometer plane. Which axis (x or y) is which does not matter for rot z.
if it is a left handed or right handed system, maybe you have to change x = -x or y =-y
*/
/*
(Gx*(Gz*(Gy*My+Gx*Mx)-fl_sqxy*Mz)*invG - Gy*(Gx*My-Gy*Mx))/
(Gy*(Gz*(Gy*My+Gx*Mx)-fl_sqxy*Mz)*invG + Gx*(Gx*My-Gy*Mx))
*/
//Gy.f=-Gy.f;
fltmp=(Gz.f*(Gy.f*My.f+Gx.f*Mx.f)-fl_sqxy*Mz.f)*invG;
/*
(Gx*fltmp - Gy*(Gx*My-Gy*Mx))/
(Gy*fltmp + Gx*(Gx*My-Gy*Mx))
*/
fl_sqxy=(Gx.f*My.f-Gy.f*Mx.f);
/*
(Gx*fltmp - Gy*fl_sqxy)/
(Gy*fltmp + Gx*fl_sqxy)
*/
// heading =_flatan2((Gy.f*fltmp + Gx.f*fl_sqxy),(Gx.f*fltmp - Gy.f*fl_sqxy)); // xy Magnetometer
heading =-_flatan2((Gx.f*fltmp - Gy.f*fl_sqxy),(Gy.f*fltmp + Gx.f*fl_sqxy)); // yx Magnetometer (HK328_board)
heading += MAG_DECLINIATION * 10; //add declination
heading = heading /10;
}
int16_t _flatan2(float y, float x)
{
float z = y / x;
int16_t a;
if ( abs(y) < abs(x) ){
a = 573 * z / (1.0f + 0.28f * z * z);
if (x<0) {
if (y<0) a -= 1800;
else a += 1800;
}
} else {
a = 900 - 573 * z / (z * z + 0.28f);
if (y<0) a -= 1800;
}
return a;
}
float _flpervatan(float z)
{
//z is inversed meaning, comes from invsqrt
float a;
if ( abs(z) < 1.0f )
{
a = 900.0f - 573.0f* z / (1.0f + 0.28f * z * z);
}
else
{
a = 573.0f * z / (z * z + 0.28f);
if (z<0) a += 1800.0f;
}
return a;
}