Page 1 of 1
GY-80 mag heading flip while rolling
Posted: Thu Jan 31, 2013 12:19 pm
by 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.
Re: GY-80 mag heading flip while rolling
Posted: Thu Jan 31, 2013 10:02 pm
by 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.cppBut was never implemented ...
Re: GY-80 mag heading flip while rolling
Posted: Fri Feb 01, 2013 12:18 am
by 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?
Re: GY-80 mag heading flip while rolling
Posted: Mon Feb 04, 2013 5:49 pm
by 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
Re: GY-80 mag heading flip while rolling
Posted: Tue Feb 05, 2013 12:05 am
by 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-tutorialHere 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
Re: GY-80 mag heading flip while rolling
Posted: Wed Feb 06, 2013 6:23 pm
by 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?
Re: GY-80 mag heading flip while rolling
Posted: Wed Feb 06, 2013 7:10 pm
by 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#p30644To 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
Re: GY-80 mag heading flip while rolling
Posted: Wed Feb 06, 2013 7:48 pm
by 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.
Re: GY-80 mag heading flip while rolling
Posted: Wed Feb 06, 2013 8:50 pm
by 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".
Re: GY-80 mag heading flip while rolling
Posted: Wed Feb 06, 2013 9:15 pm
by Hamburger
I would like to see the magneto supporting/enhancing the yaw beyond that n degrees tilt limit.
Re: GY-80 mag heading flip while rolling
Posted: Wed Feb 06, 2013 9:53 pm
by 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
Re: GY-80 mag heading flip while rolling
Posted: Thu Feb 07, 2013 12:14 am
by 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;
Re: GY-80 mag heading flip while rolling
Posted: Thu Feb 07, 2013 12:44 am
by 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.hziss you are a genious
a small test on the bench: flash size is optimized and nearly 0.5ms less. No apparent tradeoff
Re: GY-80 mag heading flip while rolling
Posted: Thu Feb 07, 2013 1:41 am
by 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;