Bug in MAG-routine?

[Olaf.J]

Hi,

the mag-routine is not working as expected with my copter. So I had a look at the sourcecode and I hope I found the reason, why big differences betweend maghold and heading result in almost no reaction to the copter.

Here's the sourcecode:

#if MAG
if (abs(rcCommand[YAW]) <70 && magMode) {
int16_t dif = heading - magHold;
if (dif <= - 180) dif += 360;
if (dif >= + 180) dif -= 360;
if ( smallAngle25 ) rcCommand[YAW] -= dif*P8[PIDMAG]/30; // 18 deg
} else magHold = heading;
#endif

If I understand it right, the magHold-value should be set to actual heading, if the rcCommand[YAW]-value will be greater than 70 - that means, if I use the Yaw-Stick at the transmitter, the magHold should be actualized. BUT...

Let's assume, I use the default P8[PIDMAG] of 40 (4.0). And the magHold is 90 degree. Due to drift/wind/any other reason, the heading changed from 90 degree to 60 degree - a really noticable difference. The Yaw-stick at the transmitter is at neutral position, the copter is at horizontal position. Then the "new" rcCommand[YAW] will be:

0+30*40/30 = 40

The resulting value is so low, this will have almost no effect to the copter (at least at my copter). So we have to increase P8[PIDMAG], e.g. to 100 (10.0). Same assuments again, the "new" rcCommand[YAW] will be:

0+30*100/30 = 100

This is a value that would result in a (really slow) YAW-movement clockwise.
But it looks like the routine will be called again, before (!) the rcCommand[YAW] will be filled with the actual YAW-stick-position from the transmitter. So the rcCommand[YAW] will be NOT 0 at next call, but it's still 100. So the magHold will be set to the actual heading (60 in this case) and the copter will not try to rotate back to the 90 degree-position, but will try to stay at 60 degree.

Has anybode good ideas to fix this issue? Perhaps an additional boolean variable like "rcCommandYAWSetByMAG" would be an idea? It could be set to true anytime the rcCommand[YAW] is set automatically by the MAG-Routine and to false when the values from the RC are read.

The new code could be:

#if MAG
if ((abs(rcCommand[YAW]) <70 && magMode) || rcCommandYAWSetByMAG) {
int16_t dif = heading - magHold;
if (dif <= - 180) dif += 360;
if (dif >= + 180) dif -= 360;
if (rcCommandYAWSetByMAG) {
if ( smallAngle25 ) rcCommand[YAW] = -dif*P8[PIDMAG]/30;
} else {
if ( smallAngle25 ) rcCommand[YAW] -= dif*P8[PIDMAG]/30 // not sure, why the stick-YAW is needed in the calculation?
}
rcCommandYAWSetByMAG = 1;
} else magHold = heading;
#endif

Not implemented/tested yet - any comments/suggestions/better ideas? And where have I to reset the rcCommandYAWSetByMAG-variable in source code to make sure it's false after the values from the RC are read?

Regards,

Olaf.

[ziojos]

I'm using this and it seem to work with different PID from default:

Code: Select all

  if(MAG) {
    #define HEADINGBAND 25 //Jos add:05/02/2012
    if (abs(rcCommand[YAW]) < HEADINGBAND && magMode) { //Jos modified 01/02/2012 (more accurate heading tenth of degrees instead degrees) band was 70
      rcCommand[YAW] = 0;  
      int32_t dif = heading - magHold;        //Jos was int16_t dif  
      if (dif <= - 1800) dif += 3600;
      if (dif >= + 1800) dif -= 3600;
      rcCommand[YAW] -= (dif*P8[PIDMAG]) >> 7;  //Jos mod: 13/03/2012 - Test to avoid drift      
    } 
    else {
      magHold = heading;
   }

I have added a digit to the heading changing in IMU.ino

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 );  //Jos more accurate heading
//   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 ) / 10;

Ciao

Josè

[Olaf.J]

Hmmm, but it looks like this routine should have the same issue - rcCommand[YAW] will possibly be set to a value greater than headingband - and then magHold will be modified, although it shouldn't. Or is it possibly an issue with the Arduino 1.0 that the else part will be executed when the condition will be changed within the then part? Sounds not very logical, but who knows...

[Alexinparis]

But it looks like the routine will be called again, before (!) the rcCommand[YAW] will be filled with the actual YAW-stick-position from the transmitter. So the rcCommand[YAW] will be NOT 0 at next call, but it's still 100. So the magHold will be set to the actual heading (60 in this case) and the copter will not try to rotate back to the 90 degree-position, but will try to stay at 60 degree.

rcCommand[YAW] is recomputed at each annexCode() call (from rcData[] which comes directly form RX.pde), ie once per cycle and before MAG compensation.
no no error for me here

[ziojos]

..... then magHold will be modified, although it shouldn't..

Why shoudn't ? I think if you enter a rcCommand[YAW] > HEADINGBAND you want change the head direction <-> you want a new magHold .

If you input a rcCommand[YAW] < HEADINGBAND it has only a little effect btw HEADINGBAND is small and this effect is neglegible in my code this is avoided at all by setting rcCommand[YAW] to zero

[Olaf.J]

rcCommand[YAW] is recomputed at each annexCode() call (from rcData[] which comes directly form RX.pde), ie once per cycle and before MAG compensation.
no no error for me here

Sounds absolutely logical - but if you add a

debug2 = magHold

at the end of the Yaw/MAG-routine and increase P-Value for MAG to 20, you will see the magHold-value jumping every 5 degree when rotating the copter manually. I had to change the code to this:

Code: Select all

  #if MAG
    if ((abs(rcCommand[YAW]) <70 && magMode) || rcCommandYAWSetByMAG) {
      int16_t dif = heading - magHold;
      if (dif <= - 180) dif += 360;
      if (dif >= + 180) dif -= 360;
      if ( smallAngle25 ) rcCommand[YAW] = -dif*P8[PIDMAG]/4;
      if (rcCommand[YAW] > 500) 
        rcCommand[YAW] = 500;
      else
        if (rcCommand[YAW] < -500) 
          rcCommand[YAW] = -500;
      rcCommandYAWSetByMAG = 1;
    } else 
      if (rcCommandYAWSetByMAG == 0) magHold = heading;
  #endif

and within the annexCode

Code: Select all

  // original Command
  rcCommand[THROTTLE] = MINTHROTTLE + (int32_t)(MAXTHROTTLE-MINTHROTTLE)* (rcData[THROTTLE]-MINCHECK)/(2000-MINCHECK);

  // additional Command by Olaf
  rcCommandYAWSetByMAG = 0;

to get it work. Don't know why...

[Alexinparis]

I tried to change

Code: Select all

  #if MAG
    if (abs(rcCommand[YAW]) <70 && magMode) {
      int16_t dif = heading - magHold;
      if (dif <= - 180) dif += 360;
      if (dif >= + 180) dif -= 360;
      if ( smallAngle25 ) rcCommand[YAW] -= dif*P8[PIDMAG]/30;  // 18 deg
    } else magHold = heading;
  #endif
debug2=magHold;

And magHold doesn't change once MAG is activated whatever the P MAG value.
Nothing seems to be wrong. please check all your code mods.

[Olaf.J]

Nothing seems to be wrong. please check all your code mods.

Seems, like nobody else has this problem with the MAG-routine. So perhaps you are right and it's an issue of any other code-modification made by me...

[JohnyGab]

I have a bug too with MAG in fact, even with mag activated, the Yaw never return to his angle. for example, let say the quad is in the air, and GYRO, ACC AND MAG active, If the head is pointing north, and I make it turn with my hand, the quad itself opose to the rotation, but once I release it with my hand, he does not go back facing north.

[Hamburger]

Same for me. Heading under mag Does not withstand gusts of wind.

[jevermeister]

Hi,
I rechecked the Mag Mode and cannot confirm a problem, if I tuzrn the copter by force it wants to return to old heading, the wider the angle gets, the more the force grows I feel.

Perhaps you should doublecheck your sensor orientation, it has changed in 2.0.

Nils

[ziojos]

Same for me. Heading under mag Does not withstand gusts of wind.

I think default MAG P is less than you need I suggest a more high value ,

more I suggest to add a bit of I and D in YAW PID to help stability

This work for me (BTW I use a more accurate angle estimation see my previous post)

Ciao

Josè

[shikra]

Have you guys successfully calibrated the MAG and verified results on the gui (both guis ?).

Mine was calibrated perfect on 1.9, but I can't seem to calibrate now. The gui indicator shows the copter moving to a certain extent, but it only goes part the way. Does not match the copter any more.

Have tried a few times and its not right.


I now think this might be related to some weird yaw issues I was getting after 2.0 - played with tx switche and think mag may have been enabled by mistake.

EDIT - tried 1.9 - cal works perfect. sensors are good - phew....
Tried 2.0 again. no joy

With a little effort.... SIRIUS IMU MAG definitions are incorrect...
Should be...

#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = X; magADC[PITCH] = Y; magADC[YAW] = -Z;}

[ziojos]

I'm on a 1.9 modified. I'm using the old style calibration that seems give good results adding declination and mounting error :

in my SW (1.9) I have this in config.h:

Code: Select all

#define MAG_MOUNTING_CORRECTION -60  //Jos add 28/03/2012 ERROR IN MAG ALLIGN ON THE BOARTD (e.g. -80 = -8°)
#define DECLINATION -27  //Jos add 19/03/2012 (2.73f = 2°44' Avellino Italy -> MWii Integer 27)

and iin imu.ino:

Code: Select all

heading += MAG_MOUNTING_CORRECTION + DECLINATION;; //Jos add 19/03/2012;    

I have tested today with a very good GPS Retur to Home

Ciao

[shikra]

Alex - if missed my edit, SIRIUS IMU definitions for mag are incorrect. May be cause of some reported yaw issues like mine. Should be.


#define MAG_ORIENTATION(X, Y, Z) {magADC[ROLL] = X; magADC[PITCH] = Y; magADC[YAW] = -Z;}