Real magnetometer calibration for GPS RTH

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
User avatar
EOSBandi
Posts: 802
Joined: Sun Jun 19, 2011 11:32 am
Location: Budapest, Hungary
Contact:

Real magnetometer calibration for GPS RTH

Post by EOSBandi »

Hi Guys,
Yesterday night I was trying to increase the precision of my compass to support RTH. Currently I experienced quite big circles around home position. I think this was caused by the missing magnetic declination which tells the FC the difference between magnetic north and geographic north. However when I started to compare my magnetometer readings with a military compass, I realized that although it’s returning to the original position precisely when turning my copter 360’ , it has a nonlinear error, which means that readings are 10’ off when pointing at East, 30’ off when pointing at South and dead on again when copter pointing at West.

This type of error does not cause any significant problem with headfree mode, but cause issues (included with the circling) when compass is used for determining heading for navigation. Reading through the code it turned out that mag calibration does not really calibrate the mag, just determine the range of mag values. Real calibration should determine magnetic field distortions caused by the surroundings and give offsets and scale factors for heading calculations.

I found a very good article about the topic recently. (See attached pdf).So I took the method described in this paper and added it to the mag calibration. Alongside with that I realized that my sensor is a little bit skewed and this cause a 6’ linear error. So I added a way to compensate that skew also. Now after a mag calibration, my readings are perfectly in sync with the copter’s orientation.
Here are the code changes, against r438.

in Multiwii.ino add some new variables :

Code: Select all

// **************
// gyro+acc IMU
// **************
static float  magXsf, magYsf;          // Mag X,Y scaling factors
static int16_t magXoff, magYoff;    // Mag X,Y offsets


Add these variables to the EEPROM
EEPROM.INO

Code: Select all

#ifdef TRI
, &tail_servo_mid,  sizeof(tail_servo_mid)
#endif
, &magXsf, sizeof(magXsf)
, &magYsf, sizeof(magYsf)
, &magXoff, sizeof(magXoff)
, &magYoff, sizeof(magYoff)}; 


And populate them in sensors.ino
in the function Mag_getADC()
Add this before the writeParams() call

Code: Select all

    
      float ftemp;

      ftemp = (magZeroTempMax[PITCH]-magZeroTempMin[PITCH])/(magZeroTempMax[ROLL]-magZeroTempMin[ROLL]);
      if (ftemp<1) { magXsf = 1; }
       else { magXsf = ftemp; } 
      ftemp = (magZeroTempMax[ROLL]-magZeroTempMin[ROLL])/(magZeroTempMax[PITCH]-magZeroTempMin[PITCH]);
      if (ftemp<1) { magYsf = 1; }
       else { magYsf = ftemp; } 
      magXoff = (( magZeroTempMax[ROLL]-magZeroTempMin[ROLL])/2 - magZeroTempMax[ROLL])*magXsf;
      magYoff = (( magZeroTempMax[PITCH]-magZeroTempMin[PITCH])/2 - magZeroTempMax[PITCH])*magYsf;
      writeParams();


and use these values in imu.ino

replace this line

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  ) / 10;

with this

Code: Select all

   float magXH,magYH;
   magYH = EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X;
   magXH = EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z;
   magYH = magYsf*magYH+magYoff;
   magXH = magXsf*magXH+magXoff;

   heading = _atan2( magYH , magXH ) / 10;
   
   #define MAG_SKEW_CORRECTION 6   
   //linear correction, caused by sensor skew related to frame
   heading = heading + MAG_SKEW_CORRECTION;
   if (heading>180) { heading = heading - 360; }
   


It is not flight tested yet, but on the desk it gives correct values. I recommend to calibrate your mag before flying out in the field...
Attachments
sae.zip
(180.72 KiB) Downloaded 426 times

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: Real magnetometer calibration for GPS RTH

Post by Alexinparis »

Hi,
Thank you again to take time to dig in this code.
I checked rapidly and it doesn't add a significant delay ;)
but doesn't work as expected on my setup. (there is still a non linear effect)

some notes:
the procedure is described for a 2 axis mag
shouldn't it be extended to the Z axis before the final X/Y calculation ?

((max-min)/2 - max)= -(max+min)/2

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

Re: Real magnetometer calibration for GPS RTH

Post by EOSBandi »

Alexinparis wrote:Hi,
Thank you again to take time to dig in this code.
I checked rapidly and it doesn't add a significant delay ;)
but doesn't work as expected on my setup. (there is still a non linear effect)

some notes:
the procedure is described for a 2 axis mag
shouldn't it be extended to the Z axis before the final X/Y calculation ?

((max-min)/2 - max)= -(max+min)/2


Nope, http://www.sensorsmag.com/sensors/motio ... fects-6475 it must be applied on X and Y axises after tilt compensation...
You are right, I just copied the equatinons, and not tired to simplify them :D

Reading through again I think It is not a good idea to do it with the original compass calibration, since calculating for soft and hard iron effects needs a complete circle but in level....

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

Re: Real magnetometer calibration for GPS RTH

Post by EOSBandi »

Hi Guys,
I reworked the mag calibration. Now it uses HMC sensors internal gain calibration for setting gains, and calculates offsets for x,y,z based on the 30sec roatation. Gain calibration is done at every init, offsets are stored in eeprom.
gain calibration for AKA sensors will come...
There is a method for calculating offsets during flight http://gentlenav.googlecode.com/files/M ... isited.pdf , but it needs a couple of vector operations which I think unnecessary overhead, so we can use the "30sec rotate your copter for calibration" method.

This is the first version of the code, so it could be optimised for memory footprint.

Changes :
in MultiWii.ino we have to change magADC to float and add three variables for offset and gain

Code: Select all

static float    magADC[3];  //This is now float for precise calculations
static float   mag_cal[3] = {1.0,1.0,1.0};  //gains for each axis, populated at sensor init (ones for AKA sensors till gain setting is not implemented)
static int16_t magXoff, magYoff, magZoff; //offsets for each axis


in sensors.ino this replaces mag functions Mag_getADC and Mag_init for HMC sensors

Code: Select all


// ************************************************************************************************************
// I2C Compass common function
// ************************************************************************************************************
#if MAG
void Mag_getADC() {
  static uint32_t t,tCal = 0;
  static int16_t magZeroTempMin[3];
  static int16_t magZeroTempMax[3];
  uint8_t axis;
  if ( currentTime < t ) return; //each read is spaced by 100ms
  t = currentTime + 100000;
  TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
  Device_Mag_getADC();

  if (calibratingM == 1) {
    tCal = t;
    for(axis=0;axis<3;axis++) {magZero[axis] = 0;magZeroTempMin[axis] = 0; magZeroTempMax[axis] = 0;}
    magXoff = 0;    //Zero out offsets
    magYoff = 0;
    magZoff = 0;
    calibratingM = 0;
  }

  magADC[ROLL]  = magADC[ROLL]*mag_cal[ROLL]+magXoff;
  magADC[PITCH] = magADC[PITCH]*mag_cal[PITCH]+magYoff;
  magADC[YAW]   = magADC[YAW]*mag_cal[YAW]+magZoff;

  if (tCal != 0) {
    if ((t - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions
      LEDPIN_TOGGLE;
      for(axis=0;axis<3;axis++) {
        if (magADC[axis] < magZeroTempMin[axis]) magZeroTempMin[axis] = magADC[axis];
        if (magADC[axis] > magZeroTempMax[axis]) magZeroTempMax[axis] = magADC[axis];
      }
    } else {
      tCal = 0;
      for(axis=0;axis<3;axis++)
        magZero[axis] = (( magZeroTempMax[axis]-magZeroTempMin[axis])/2 - magZeroTempMax[axis]); //Calculate offsets
      magXoff = magZero[ROLL];    //Store them
      magYoff = magZero[PITCH];
      magZoff = magZero[YAW]; 
       
      writeParams();
    }
  }
}
#endif

// ************************************************************************************************************
// I2C Compass HMC5843 & HMC5883
// ************************************************************************************************************
// I2C adress: 0x3C (8bit)   0x1E (7bit)
// ************************************************************************************************************
#if defined(HMC5843) || defined(HMC5883)

#define COMPASS_ADDRESS      0x3C
#define ConfigRegA           0x00
#define ConfigRegB           0x01
#define magGain              0x20
#define PositiveBiasConfig   0x11
#define NegativeBiasConfig   0x12
#define NormalOperation      0x10
#define ModeRegister         0x02
#define ContinuousConversion 0x00
#define SingleConversion     0x01

// ConfigRegA valid sample averaging for 5883L
#define SampleAveraging_1    0x00
#define SampleAveraging_2    0x01
#define SampleAveraging_4    0x02
#define SampleAveraging_8    0x03

// ConfigRegA valid data output rates for 5883L
#define DataOutputRate_0_75HZ 0x00
#define DataOutputRate_1_5HZ  0x01
#define DataOutputRate_3HZ    0x02
#define DataOutputRate_7_5HZ  0x03
#define DataOutputRate_15HZ   0x04
#define DataOutputRate_30HZ   0x05
#define DataOutputRate_75HZ   0x06


void Mag_init() {
  //Calibration code is based on code written by Jordi Muñoz and Jose Julio from diydrones.com
  int numAttempts = 0, good_count = 0;
  bool success = false;
  byte calibration_gain = 0x20;
  uint16_t expected_x = 715;      //default values for HMC5843
  uint16_t expected_yz = 715;
  float gain_multiple = 1.0;

  delay(100);

  //Setup
  i2c_writeReg(COMPASS_ADDRESS,ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation);     

 #if defined (HMC5883)      //These values for HMC5883
  calibration_gain = 0x60;
  expected_x = 766;
  expected_yz  = 713;
  gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain
 #endif

  mag_cal[0] = 0;    //Zero out the offsets
  mag_cal[1] = 0;
  mag_cal[2] = 0;
 
  while ( success == 0 && numAttempts < 20 && good_count < 5)
  {
      // record number of attempts at initialisation
     numAttempts++;

     // force positiveBias (compass should return 715 for all channels)
     i2c_writeReg(COMPASS_ADDRESS,ConfigRegA, PositiveBiasConfig);
     delay(50);
     // set gains for calibration
     i2c_writeReg(COMPASS_ADDRESS,ConfigRegB, calibration_gain);
                  i2c_writeReg(COMPASS_ADDRESS,ModeRegister, SingleConversion);

     // read values from the compass no offset not calibration
     delay(50);
                  Device_Mag_getADC();
     delay(10);

     float cal[3];

     cal[0] = fabs(expected_x / (float)magADC[ROLL]);
     cal[1] = fabs(expected_yz / (float)magADC[PITCH]);
     cal[2] = fabs(expected_yz / (float)magADC[ROLL]);

     if (cal[0] > 0.7 && cal[0] < 1.3 &&
        cal[1] > 0.7 && cal[1] < 1.3 &&
        cal[2] > 0.7 && cal[2] < 1.3) {
       good_count++;
       mag_cal[0] += cal[0];
       mag_cal[1] += cal[1];
       mag_cal[2] += cal[2];
     }
  }

  if (good_count >= 5) {
    mag_cal[0] = mag_cal[0] * gain_multiple / good_count;
    mag_cal[1] = mag_cal[1] * gain_multiple / good_count;
    mag_cal[2] = mag_cal[2] * gain_multiple / good_count;
    success = true;
  } else {
    /* best guess */
    mag_cal[0] = 1.0;
    mag_cal[1] = 1.0;
    mag_cal[2] = 1.0;
  }

  // leave test mode
  i2c_writeReg(COMPASS_ADDRESS,ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation);     
  i2c_writeReg(COMPASS_ADDRESS,ConfigRegB, magGain);
  i2c_writeReg(COMPASS_ADDRESS,ModeRegister, ContinuousConversion);
 
  //We have offsets loaded from eeprom in magZero[] move them to mag.off variables (quick hack for not disturbing eeprom structure)
  magXoff = magZero[ROLL]; 
  magYoff = magZero[PITCH];
  magZoff = magZero[YAW];   

}


sandmen
Posts: 24
Joined: Mon Feb 21, 2011 3:26 pm

Re: Real magnetometer calibration for GPS RTH

Post by sandmen »

Hi,
I want to try your changes but for me it doesn't work.
I think it Hangs on the MAG_Init
Because have only Blinking LED, like not finished the init.
I have the HMC5843.

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

Re: Real magnetometer calibration for GPS RTH

Post by EOSBandi »

Well, I did not tested it wi 5843, I think I have one somewhere, I'll dig it out and will test it...

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

Re: Real magnetometer calibration for GPS RTH

Post by EOSBandi »

Hi, found a 5843 blob, and it seems to work. If you undefine HMC then it starts ?

sandmen
Posts: 24
Joined: Mon Feb 21, 2011 3:26 pm

Re: Real magnetometer calibration for GPS RTH

Post by sandmen »

I will test it this evening.
Thanks

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

Re: Real magnetometer calibration for GPS RTH

Post by nhadrian »

I can confirm that with HMC5843 it works.

BR
Adrian

alexmos
Posts: 108
Joined: Tue Aug 09, 2011 12:12 pm

Re: Real magnetometer calibration for GPS RTH

Post by alexmos »

Hi! Good job, I am waiting to see it in next release! Please take a look on my ideas to improve: in some setups magnetometer is too close to power wires (battery, ESC). When they fully loaded with current (30 Amps for general setup) it makes a noticable inflation on the magnetometer. So, I think it should be 'calibration on the fly' option, or any case of auto correction.

MikSam
Posts: 10
Joined: Wed Feb 22, 2012 5:27 am

Re: Real magnetometer calibration for GPS RTH

Post by MikSam »

Confirmed working with HMC5883L.
Good job!

- Mik

User avatar
jevermeister
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am
Contact:

Re: Real magnetometer calibration for GPS RTH

Post by jevermeister »

Hi,
is there any reason Alex Changed the Mag Orientation?

I had some trouble using the orientation change in config.h until I noticed, that someone did a change in the serial.pde

Nils

Alexinparis
Posts: 1630
Joined: Wed Jan 19, 2011 9:07 pm

Re: Real magnetometer calibration for GPS RTH

Post by Alexinparis »

jevermeister wrote:Hi,
is there any reason Alex Changed the Mag Orientation?

I had some trouble using the orientation change in config.h until I noticed, that someone did a change in the serial.pde

Nils

orientation code was changed in sensors.pde
it impacts individual sensors config. => you need to redefine your orientation.
it should not impact predefined boards (except some remaining errors)

User avatar
jevermeister
Posts: 708
Joined: Wed Jul 20, 2011 8:56 am
Contact:

Re: Real magnetometer calibration for GPS RTH

Post by jevermeister »

Hi,

already done that, it is working now.

As far as 'I can say, the change of orientation is now correct. It was wrong beforce concerning my HMC5883L

Nils

timecop
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Real magnetometer calibration for GPS RTH

Post by timecop »

Did anything ever happen with this? At some point I saw some "better" mag calibration with a 5-take calibration loop but its nowhere to be found. A better tilt compensation would be nice as well, as was posted in few earlier posts.

User avatar
shikra
Posts: 783
Joined: Wed Mar 30, 2011 7:58 pm

Re: Real magnetometer calibration for GPS RTH

Post by shikra »

Not sure if this is what you are after, but the physical method in first post here advise a better way to calibrate. I tried and could see a difference. Mag is more accurate in response and less impacted by changes in orientation. At least for me at my location. I am not really a Mag user in normal flying, but I happier to know its set better for when using GPS hold/rth
viewtopic.php?f=8&t=1387&p=10658

timecop
Posts: 1880
Joined: Fri Sep 02, 2011 4:48 pm

Re: Real magnetometer calibration for GPS RTH

Post by timecop »

Yes, but I'm also talking about this:
http://code.google.com/p/multiwii-gps-w ... s.ino#1001

This disapearred when this project became i2c-gps-nav and eventually got merged.

EDCKiwi
Posts: 2
Joined: Sun Dec 09, 2012 2:56 am

Re: Real magnetometer calibration for GPS RTH

Post by EDCKiwi »

EOSBandi,
I have been trying to get this working, got nothing better than I started with. You post from Jan 6th is all the code changes required right? Not the earlier portion of code?

Can you post the complete files that you changed please, rather than having to locate the section in code.

I had a 90 offset that I wanted to alter and had thought perhaps this would fix that for me.

Thank you,
Edan

Post Reply