Compass calculations

Firmware
1

So ive been playing around with the sparkfun IMU and trying to get a decent pitch, roll and yaw output. there is a function called printAttitude which should do it, but the heading (yaw) doesn’t seem to work correctly. the pitch and roll are perfect but the heading changes but only goes from about 110 to 230??? and i dont think its tilt compensated at all…

anyway so i found this article and wanted to implement it in the particle ecosystem http://cache.freescale.com/files/sensors/doc/app_note/AN4248.pdf but its way way way over my head

the code compiles and i get very similar results… would someone mind taking a look over it for me? the LSM9DS1 libs are in the webIDE, i just cut and pasted them into my own tabs

#include "application.h"
#include "SparkFunLSM9DS1.h"
#include "LSM9DS1_Registers.h"
#include "LSM9DS1_Types.h"
#include "math.h"

LSM9DS1 imu;

#define LSM9DS1_M    0x1E 
#define LSM9DS1_AG    0x6B 

//this code is derived from http://cache.freescale.com/files/sensors/doc/app_note/AN4248.pdf

/* roll pitch and yaw angles computed by iecompass */
static int16_t iPhi, iThe, iPsi;
/* magnetic field readings corrected for hard iron effects and PCB orientation */
static int16_t iBfx, iBfy, iBfz;
/* hard iron estimate */
static int16_t iVx, iVy, iVz;
/* tilt-compensated e-Compass code */

const uint16_t MINDELTATRIG = 1; /* final step size for iTrig */
/* function to calculate ir = ix / sqrt(ix*ix+iy*iy) using binary division */

const uint16_t MINDELTADIV = 1; /* final step size for iDivide */

/* fifth order of polynomial approximation giving 0.05 deg max error */
const int16_t K1 = 5701;
const int16_t K2 = -1645;
const int16_t K3 = 446;



void setup() {
    imu.settings.device.commInterface = IMU_MODE_I2C;
    imu.settings.device.mAddress = LSM9DS1_M;
    imu.settings.device.agAddress = LSM9DS1_AG;
    
    iVx = 0;
    iVy = 0;
    iVz = 0;
    
    Serial.begin(115200);
    Serial.println("eCompass Test App");
    
    imu.begin();

}

void loop() {
    imu.readMag();
    imu.readAccel();
    
    iecompass(-imu.my, -imu.mx, imu.mz, imu.ax, imu.ay, imu.az);
    //iPhi, iThe, iPsi
    Serial.print("Pitch = ");
    Serial.println(iPhi / 100.0f);
    Serial.print("Roll  = ");
    Serial.println(iThe / 100.0f);
    Serial.print("Yaw   = ");
    Serial.println(iPsi / 100.0f);    
    Serial.println("");
    delay(1000);

}



static void iecompass(int16_t iBpx, int16_t iBpy, int16_t iBpz, int16_t iGpx, int16_t iGpy, int16_t iGpz)
{
    /* stack variables */
    /* iBpx, iBpy, iBpz: the three components of the magnetometer sensor */
    /* iGpx, iGpy, iGpz: the three components of the accelerometer sensor */
    /* local variables */
    int16_t iSin, iCos; /* sine and cosine */
    /* subtract the hard iron offset */
    iBpx -= iVx; /* see Eq 16 */
    iBpy -= iVy; /* see Eq 16 */
    iBpz -= iVz; /* see Eq 16 */
    /* calculate current roll angle Phi */
    iPhi = iHundredAtan2Deg(iGpy, iGpz);/* Eq 13 */
    /* calculate sin and cosine of roll angle Phi */
    iSin = iTrig(iGpy, iGpz); /* Eq 13: sin = opposite / hypotenuse */
    iCos = iTrig(iGpz, iGpy); /* Eq 13: cos = adjacent / hypotenuse */
    /* de-rotate by roll angle Phi */
    iBfy = (int16_t)((iBpy * iCos - iBpz * iSin) >> 15);/* Eq 19 y component */
    iBpz = (int16_t)((iBpy * iSin + iBpz * iCos) >> 15);/* Bpy*sin(Phi)+Bpz*cos(Phi)*/
    iGpz = (int16_t)((iGpy * iSin + iGpz * iCos) >> 15);/* Eq 15 denominator */
    /* calculate current pitch angle Theta */
    iThe = iHundredAtan2Deg((int16_t)-iGpx, iGpz);/* Eq 15 */
    /* restrict pitch angle to range -90 to 90 degrees */
    if (iThe > 9000) iThe = (int16_t) (18000 - iThe);
    if (iThe < -9000) iThe = (int16_t) (-18000 - iThe);
    /* calculate sin and cosine of pitch angle Theta */
    iSin = (int16_t)-iTrig(iGpx, iGpz); /* Eq 15: sin = opposite / hypotenuse */
    iCos = iTrig(iGpz, iGpx); /* Eq 15: cos = adjacent / hypotenuse */
    /* correct cosine if pitch not in range -90 to 90 degrees */
    if (iCos < 0) iCos = (int16_t)-iCos;
    /* de-rotate by pitch angle Theta */
    iBfx = (int16_t)((iBpx * iCos + iBpz * iSin) >> 15); /* Eq 19: x component */
    iBfz = (int16_t)((-iBpx * iSin + iBpz * iCos) >> 15);/* Eq 19: z component */
    /* calculate current yaw = e-compass angle Psi */
    iPsi = iHundredAtan2Deg((int16_t)-iBfy, iBfx); /* Eq 22 */
}

/*
int32_t tmpAngle; // temporary angle*100 deg: range -36000 to 36000 
static int16_t iLPPsi; // low pass filtered angle*100 deg: range -18000 to 18000 
static uint16_t ANGLE_LPF; // low pass filter: set to 32768 / N for N samples averaging 
// implement a modulo arithmetic exponential low pass filter on the yaw angle 
// compute the change in angle modulo 360 degrees 
tmpAngle = (int32_t)iPsi - (int32_t)iLPPsi;
if (tmpAngle > 18000) tmpAngle -= 36000;
if (tmpAngle < -18000) tmpAngle += 36000;
// calculate the new low pass filtered angle 
tmpAngle = (int32_t)iLPPsi + ((ANGLE_LPF * tmpAngle) >> 15);
// check that the angle remains in -180 to 180 deg bounds 
if (tmpAngle > 18000) tmpAngle -= 36000;
if (tmpAngle < -18000) tmpAngle += 36000;
// store the correctly bounded low pass filtered angle 
iLPPsi = (int16_t)tmpAngle;

//For the pitch angle θ, which is restricted to the range -90° to 90°, the final bounds check should be changed to:
if (tmpAngle > 9000) tmpAngle = (int16_t) (18000 - tmpAngle);
if (tmpAngle < -9000) tmpAngle = (int16_t) (-18000 - tmpAngle);


*/


static int16_t iTrig(int16_t ix, int16_t iy)
{
    uint32_t itmp; /* scratch */
    uint32_t ixsq; /* ix * ix */
    int16_t isignx; /* storage for sign of x. algorithm assumes x >= 0 then corrects later */
    uint32_t ihypsq; /* (ix * ix) + (iy * iy) */
    int16_t ir; /* result = ix / sqrt(ix*ix+iy*iy) range -1, 1 returned as signed Int16 */
    int16_t idelta; /* delta on candidate result dividing each stage by factor of 2 */
    /* stack variables */
    /* ix, iy: signed 16 bit integers representing sensor reading in range -32768 to 32767 */
    /* function returns signed Int16 as signed fraction (ie +32767=0.99997, -32768=-1.0000) */
    /* algorithm solves for ir*ir*(ix*ix+iy*iy)=ix*ix */
    /* correct for pathological case: ix==iy==0 */
    if ((ix == 0) && (iy == 0)) ix = iy = 1;
    /* check for -32768 which is not handled correctly */
    if (ix == -32768) ix = -32767;
    if (iy == -32768) iy = -32767;
    /* store the sign for later use. algorithm assumes x is positive for convenience */
    isignx = 1;
    if (ix < 0)
    {
        ix = (int16_t)-ix;
        isignx = -1;
    }
    /* for convenience in the boosting set iy to be positive as well as ix */
    iy = (int16_t)abs(iy);
    /* to reduce quantization effects, boost ix and iy but keep below maximum signed 16 bit */
    while ((ix < 16384) && (iy < 16384))
    {
        ix = (int16_t)(ix + ix);
        iy = (int16_t)(iy + iy);
    }
    /* calculate ix*ix and the hypotenuse squared */
    ixsq = (uint32_t)(ix * ix); /* ixsq=ix*ix: 0 to 32767^2 = 1073676289 */
    ihypsq = (uint32_t)(ixsq + iy * iy); /* ihypsq=(ix*ix+iy*iy) 0 to 2*32767*32767=2147352578 */
    /* set result r to zero and binary search step to 16384 = 0.5 */
    ir = 0;
    idelta = 16384; /* set as 2^14 = 0.5 */
    /* loop over binary sub-division algorithm */
    do
    {
    /* generate new candidate solution for ir and test if we are too high or too low */
    /* itmp=(ir+delta)^2, range 0 to 32767*32767 = 2^30 = 1073676289 */
        itmp = (uint32_t)((ir + idelta) * (ir + idelta));
        /* itmp=(ir+delta)^2*(ix*ix+iy*iy), range 0 to 2^31 = 2147221516 */
        itmp = (itmp >> 15) * (ihypsq >> 15);
        if (itmp <= ixsq) ir += idelta;
        idelta = (int16_t)(idelta >> 1); /* divide by 2 using right shift one bit */
    } while (idelta >= MINDELTATRIG); /* last loop is performed for idelta=MINDELTATRIG */
    /* correct the sign before returning */
    return (int16_t)(ir * isignx);
}


/* calculates 100*atan2(iy/ix)=100*atan2(iy,ix) in deg for ix, iy in range -32768 to 32767 */
static int16_t iHundredAtan2Deg(int16_t iy, int16_t ix)
{
    int16_t iResult; /* angle in degrees times 100 */
    /* check for -32768 which is not handled correctly */
    if (ix == -32768) ix = -32767;
    if (iy == -32768) iy = -32767;
    /* check for quadrants */
    if ((ix >= 0) && (iy >= 0)) /* range 0 to 90 degrees */
        iResult = iHundredAtanDeg(iy, ix);
    else if ((ix <= 0) && (iy >= 0)) /* range 90 to 180 degrees */
        iResult = (int16_t)(18000 - (int16_t)iHundredAtanDeg(iy, (int16_t)-ix));
    else if ((ix <= 0) && (iy <= 0)) /* range -180 to -90 degrees */
        iResult = (int16_t)((int16_t)-18000 + iHundredAtanDeg((int16_t)-iy, (int16_t)-ix));
    else /* ix >=0 and iy <= 0 giving range -90 to 0 degrees */
        iResult = (int16_t)(-iHundredAtanDeg((int16_t)-iy, ix));
    return (iResult);
}



/* calculates 100*atan(iy/ix) range 0 to 9000 for all ix, iy positive in range 0 to 32767 */
static int16_t iHundredAtanDeg(int16_t iy, int16_t ix)
{
    int32_t iAngle; /* angle in degrees times 100 */
    int16_t iRatio; /* ratio of iy / ix or vice versa */
    int32_t iTmp; /* temporary variable */
    /* check for pathological cases */
    if ((ix == 0) && (iy == 0)) return (0);
    if ((ix == 0) && (iy != 0)) return (9000);
    /* check for non-pathological cases */
    if (iy <= ix)
    iRatio = iDivide(iy, ix); /* return a fraction in range 0. to 32767 = 0. to 1. */
    else
    iRatio = iDivide(ix, iy); /* return a fraction in range 0. to 32767 = 0. to 1. */
    /* first, third and fifth order polynomial approximation */
    iAngle = (int32_t) K1 * (int32_t) iRatio;
    iTmp = ((int32_t) iRatio >> 5) * ((int32_t) iRatio >> 5) * ((int32_t) iRatio >> 5);
    iAngle += (iTmp >> 15) * (int32_t) K2;
    iTmp = (iTmp >> 20) * ((int32_t) iRatio >> 5) * ((int32_t) iRatio >> 5);
    iAngle += (iTmp >> 15) * (int32_t) K3;
    iAngle = iAngle >> 15;
    /* check if above 45 degrees */
    if (iy > ix) iAngle = (int16_t)(9000 - iAngle);
    /* for tidiness, limit result to range 0 to 9000 equals 0.0 to 90.0 degrees */
    if (iAngle < 0) iAngle = 0;
    if (iAngle > 9000) iAngle = 9000;
    return ((int16_t) iAngle);
}



/* function to calculate ir = iy / ix with iy <= ix, and ix, iy both > 0 */
static int16_t iDivide(int16_t iy, int16_t ix)
{
    int16_t itmp; /* scratch */
    int16_t ir; /* result = iy / ix range 0., 1. returned in range 0 to 32767 */
    int16_t idelta; /* delta on candidate result dividing each stage by factor of 2 */
    /* set result r to zero and binary search step to 16384 = 0.5 */
    ir = 0;
    idelta = 16384; /* set as 2^14 = 0.5 */
    /* to reduce quantization effects, boost ix and iy to the maximum signed 16 bit value */
    while ((ix < 16384) && (iy < 16384))
    {
        ix = (int16_t)(ix + ix);
        iy = (int16_t)(iy + iy);
    }
    /* loop over binary sub-division algorithm solving for ir*ix = iy */
    do
    {
    /* generate new candidate solution for ir and test if we are too high or too low */
        itmp = (int16_t)(ir + idelta); /* itmp=ir+delta, the candidate solution */
        itmp = (int16_t)((itmp * ix) >> 15);
        if (itmp <= iy) ir += idelta;
            idelta = (int16_t)(idelta >> 1); /* divide by 2 using right shift one bit */
    } while (idelta >= MINDELTADIV); /* last loop is performed for idelta=MINDELTADIV */
    return (ir);
}
1 Like
2

Hello,
I too have the same problem. I’ll have more resolved?
The N of 32768 / N in the low-pass filter what ??
did you understand ??

1 Like
3

@niki I ended up going back to the standard sparkfun code (in the IDE libraries), and ran the calibrate mag routine, that fixed the issue for me. just add this to your code, when it says started move the device though a few full rotations and it should sort itself out. it saves it into the IMU so once its done once you should be alright.

  Serial.println("calibration started");
  imu.calibrateMag(1);
  Serial.println("Calibration fininshed");

this is the function it calls in the lib. if you need more time to calibrate try changing the loop from 128 to a higher number. i ended up going to 1000, this took about 15sec to complete but will give much better results. (you will probably have to add the library manually to be able to change it i.e. copy the files to new tabs manually)

void LSM9DS1::calibrateMag(bool loadIn)
{
	int i, j;
	int16_t magMin[3] = {0, 0, 0};
	int16_t magMax[3] = {0, 0, 0}; // The road warrior
	
	for (i=0; i<128; i++)
	{
		while (!magAvailable())
			;
		readMag();
		int16_t magTemp[3] = {0, 0, 0};
		magTemp[0] = mx;		
		magTemp[1] = my;
		magTemp[2] = mz;
		for (j = 0; j < 3; j++)
		{
			if (magTemp[j] > magMax[j]) magMax[j] = magTemp[j];
			if (magTemp[j] < magMin[j]) magMin[j] = magTemp[j];
		}
	}
	for (j = 0; j < 3; j++)
	{
		mBiasRaw[j] = (magMax[j] + magMin[j]) / 2;
		mBias[j] = calcMag(mBiasRaw[j]);
		if (loadIn)
			magOffset(j, mBiasRaw[j]);
	}
	
}
3 Likes
4

Thanks for the reply.
But I do not see the calcMag () and magOffset (). Where are they??
I can not find the complete code SparkFun? You can put the link?

5

if you click the library tab on the web IDE then search for LSM9DS1 it should come up.

here is the link to it on github

2 Likes
6

Sorry for the inconvenience,
but I do not understand how mBias are used in the Freescale algorithm.
The algorithm of Freescale plans to calculate the correction for the hard iron through the magnetic field values obtained values of V.
The algorithm considers the values of magnetic field, the acceleration and V to compute the three angles.
The final low pass filter allows to reduce the measuring range -180 ° <> 180 °.

The LSM9DS1 :: calibrateMag () with magOffset transmits the magnetometer values.
And so it seems strange … ??? …

mBias the value as entered in the algorithm of Freescale?

7

I’m not 100% sure but my understanding is there is a memory area in the 9ds1 that holds the bias. That is like the centre point of magnetic flux, the values read from the device are + or - from that bias value.

The reason I was using the freescale code was because the values I was seeing were way off, in the end I didn’t use it because I found that calibrating was all that was needed

2 Likes
8

ok ok, I thought that you had joined the two codes in a third code.
So you used only LSM9DS1 or you have modified?
you can send me by email an export of your program ??
So (hopefully) I take off my doubts.

9

the code on the web ide works fine, just add the 3 lines above to calibrate the mag somewhere near the end of setup();

the code im using has been modified significantly now to run on Bluz and play nicely with other libraries so wouldn’t be much use to you.