Magnetometer+Tilt

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
Post Reply
Paulmann
Posts: 13
Joined: Thu Mar 21, 2013 11:54 pm

Magnetometer+Tilt

Post by Paulmann »

Hi,

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;
}
   

kataventos
Posts: 702
Joined: Sun Aug 28, 2011 8:14 pm
Contact:

Re: Magnetometer+Tilt

Post by kataventos »

Hi Paulmann,

if your calculations are correct, this is the explanation I was looking for! I have changed the OSD code 3 times regarding this problem (Home Arrow).
The results are "OK" now, but I am still not happy and still did not understood why do I have to change what is correct... until now!
Yes, I think that you found what is causing this issue and I will test your lines on MWii 2.2 with the old OSD heading code.

Thanks and fly safe.
Cheers,
KV

User avatar
EOSBandi
Posts: 802
Joined: Sun Jun 19, 2011 11:32 am
Location: Budapest, Hungary
Contact:

Re: Magnetometer+Tilt

Post by EOSBandi »

Yepp, this is the textbook calculation, and indeed this is as correct as it could be. The only question is that how mutch it adds to the cycletime? I think the inperfections of the currect implementation is coming from the simplifcation and approximation that Alex used to streamline the code as mutch as possible. DId you check your code in-flight ? What is your average cycletime ?

Don't get me wrong. The code above is a must for Baselight and other high processor powered boards, the question is, how it fit's to a 328 based board...
(will try this week)

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Magnetometer+Tilt

Post by nhadrian »

EOSBandi wrote:Yepp, this is the textbook calculation, and indeed this is as correct as it could be. The only question is that how mutch it adds to the cycletime? I think the inperfections of the currect implementation is coming from the simplifcation and approximation that Alex used to streamline the code as mutch as possible. DId you check your code in-flight ? What is your average cycletime ?

Don't get me wrong. The code above is a must for Baselight and other high processor powered boards, the question is, how it fit's to a 328 based board...
(will try this week)


A possible way could be to split the imu calculations with defines for different controllers.
If defined Promini and lower proc, use a simpe calculation, else use a more detailed and accuracy one.

This type of constraintwith 328p is quite annoying for me... especially because tons of cheap mega based boards are on the market.

BR Adrian

User avatar
Crashpilot1000
Posts: 631
Joined: Tue Apr 03, 2012 7:38 pm

Re: Magnetometer+Tilt

Post by Crashpilot1000 »

Wasn't that resolved with this: viewtopic.php?f=8&t=3031&start=10
Or the current 2.2 implementation from alex?

Post Reply