GY-80 mag heading flip while rolling

[pito]

Hi, playing with v213 and GY-80. While making a full-roll-only and the heading is W or E the heading (as shown on multiwiiconf compass) stays ok during the roll, but when the heading is N or S and I make the full-roll-only the heading flips during the roll to an opposite direction. Is that a feature or a bug? Thanks.

[Sebbi]

That's a bug caused by the way orientation is calculated

Code: Select all

    heading = _atan2( EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X , EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z  );

A tilt compensated heading would be calculated similar to this

Code: Select all

Xh = magADC[0] * cos(Pitch) + magADC[2] * sin(Pitch)
Yh = magADC[0] * sin(Roll) * sin(Pitch) + magADC[1] * cos(Roll) - magADC[2] * sin(Roll) * cos(Pitch) 
heading = atan2(Yh, Xh)

Or something like what is done in this arduino library https://github.com/pololu/LSM303/blob/m ... LSM303.cpp

But was never implemented ...

[weisseruebe]

That's interesting, since I still have problems with pitching the copter along the W-E Axis. Mag heading is always drifting away.

Seems like that code snippet works in WiiCopter-Code. Are you using it like that?

[Crashpilot1000]

Hi!

I tried to implement the formula but i am too stupid can someone tell me what is wrong with it?

Code: Select all

  #define deg2rad(x) (float)(x*PI/1800.0f)
  float sr = sin(deg2rad(angle[ROLL]));
  float sp = sin(deg2rad(angle[PITCH]));
  float cp = cos(deg2rad(angle[PITCH]));
  float cr = cos(deg2rad(angle[ROLL]));
  
  float heading2 = _atan2(magADC[0] * sr * sp + magADC[1] * cr - magADC[2] * sr * cp, magADC[0] * cp + magADC[2] * sp);
debug[0] = heading2;

Cheers
Kraut Rob

[Crashpilot1000]

Hi!
Finally i got it working somehow, but i doubt that it is really better than the existing code.... and there is still a problem with the code.
Sebbis' info is here: http://www.loveelectronics.co.uk/Tutori ... o-tutorial
Here is the working code: (taken from the download and adapted for mwii)

Code: Select all

  #define RADX10 0.001745329252f
  float rollRAD = (float)angle[ROLL] * RADX10;
  float pitchRAD = (float)-angle[PITCH] * RADX10;
// We cannot correct for tilt over 40 degrees with this algorthem, if the board is tilted as such, return 0.
//if(rollRAD > 0.78f || rollRAD < -0.78f || pitchRAD > 0.78f || pitchRAD < -0.78) return 0;
  float cr = cos(rollRAD);
  float sr = sin(rollRAD);
  float cp = cos(pitchRAD);
  float sp = sin(pitchRAD);
  heading = _atan2(magADC[1]*sr*sp + magADC[0]*cr - magADC[2]*sr*cp,magADC[1]*cp + magADC[2]*sp)*-1;

Cheers
Kraut Rob

[Hamburger]

so does this mod improve on the magneto usage in real life? Could the former limitation of mag to small tilt angles be removed then?

[Crashpilot1000]

Hi, Hamburger!
I found the solution here for the Naze32: viewtopic.php?f=23&t=1947&start=170#p30610 and here viewtopic.php?f=23&t=1947&start=170#p30644

To make it work on multiwii you will also have to replace this in imu:

Code: Select all

    angle[ROLL] = _atan2(EstG.V.X, EstG.V.Z);
    angle[PITCH] = _atan2(EstG.V.Y, EstG.V.Z);

with that:

Code: Select all

    angle[ROLL] = _atan2(EstG.V.X, EstG.V.Z);
    angle[PITCH] = -asin(EstG.V.Y / -sqrt(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)) * (1800.0f / PI);

I haven't tested it on mwii but it will/should work. Check the naze code - link on _atan2f or so and replace it with _atan2 etc.. M_PI is PI..... but should be no obstacle.

Cheers
Kraut Rob

[Hamburger]

interesting. Did you fly that mod to mag, how does it go?. So obviously there is no regular backport from naze32, was to be expected.

[Crashpilot1000]

Headfree still works i wasn't able to test more behind the house. The idea behind increasing mag precision is to improve gps functions and perhaps get an accurate ACC translated into earth frame. So this is only one step and Eos Bandi did the other by improving the mag calibration. Both steps together could lead to some basic "INS GPS".

[Hamburger]

I would like to see the magneto supporting/enhancing the yaw beyond that n degrees tilt limit.

[Crashpilot1000]

Hi Hamburger!
I still have my mwii board (not on a copter but anyways....)
I did a quick port for the original mwii 2.1 imu.ino. It is not optimized the sqrt and atan2 can be optimized.
So no mag gimbal lock anymore

Original snip

Code: Select all

      EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR  + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
  #endif

...
  
  // Attitude of the estimated vector
  angle[ROLL]  =  _atan2(EstG.V.X , EstG.V.Z) ;
  angle[PITCH] =  _atan2(EstG.V.Y , EstG.V.Z) ;

  #if MAG
    // Attitude of the cross product vector GxM
    heading = _atan2( 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 += MAG_DECLINIATION * 10; //add declination
    heading = heading /10;
    if ( heading > 180)      heading = heading - 360;
    else if (heading < -180) heading = heading + 360;
  #endif
}
...

#define UPDATE_INTERVAL 25000    // 40hz update rate (20hz LPF on acc)
#d

Replace the code between the "..." with this:

Code: Select all

  #define RADX10    0.001745329252f                  // PI/1800.0f
  #define RAD2DEG10 572.957795130823f                //1800.0f / PI

  // Attitude of the estimated vector
  angle[ROLL] = _atan2(EstG.V.X, EstG.V.Z);
  angle[PITCH] = -asin(EstG.V.Y / -sqrt(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)) * RAD2DEG10;
 
  #if MAG
  float rollRAD = (float)angle[ROLL] * RADX10;    // Crashpilot CORRECT MAG TILT COMPENSATION now you can fly on your back with correct heading
  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 = cos(rollRAD);
  float sr = sin(rollRAD);
  float cp = cos(pitchRAD);
  float sp = sin(pitchRAD);
  float Xh = magX * cp + magY * sr * sp + magZ * cr * sp;
  float Yh = magY * cr - magZ * sr;
  float flthead = (atan2(-Yh,Xh) * RAD2DEG10 + MAG_DECLINIATION)/10;
  if (flthead > 180.0f) flthead = flthead - 360.0f;
  else if (flthead < -180.0f) flthead = flthead + 360.0f;
  heading = flthead;
  #endif
}

It's just a starting point but it works. You can fly on your back now without loosing the heading.
See for yourself

Cheers
Kraut Rob

[howardhb]

Kraut Rob, be aware (in your code) that the magnetic declination is divided by 10.

Code: Select all

float flthead = (atan2(-Yh,Xh) * RAD2DEG10 + MAG_DECLINIATION)/10;

For correct declination:

Code: Select all

float flthead = (atan2(-Yh,Xh) * RAD2DEG10 + (MAG_DECLINIATION)*10)/10;

[Alexinparis]

Hi,

It could be much more optimized.
look how the code master avoids sin & cos arithmetic
https://code.google.com/p/mwc-ng/source ... src/AHRS.h
ziss you are a genious

a small test on the bench: flash size is optimized and nearly 0.5ms less. No apparent tradeoff

[Crashpilot1000]

@howardhb: Thank you very much for the hint! Naze already has the*10 in MAG_DECLINIATION, but i will look into this, if everything is correct there.
@Alexinparis: Great Project and great link!! So why isn't it in the official branch already??
Cheers
Kraut Rob

Edit:
Naze already delivers the Heading as Heading*10. That is the difference between mwii/naze Version. Thanks for the catch Howard!

Here is the nazecode that is done before.

Code: Select all

    deg = cfg.mag_declination / 100;
    min = cfg.mag_declination % 100;
    magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10;