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...