[ahogrelius]

I hacked the old version of berryIMU.py (Used in Raspberry Pi Embedded Cap With GPS) to work with the new version of BerryGPS-IMU and it gives pretty much the same result as the code above.

[ahogrelius]

I will try adding the shell code snippet to update the refresh every time I boot the Raspberry.

Regarding the second question. I'm using the "compass_tutorial02_tilt_compensation" example.

I was able to get stable and reliable readings by adding code to do an Exponential Moving Average of the heading. I also added code to allow calibration which improved things even more. With the changes I get stable readings that only is off by a degree or so compared to the compass app on my cellphone.

#include <unistd.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "IMU.c"

//Defines for calibration from the calibration application
#define magXmax 1177
#define magYmax 993
#define magZmax 839
#define magXmin -692
#define magYmin -945
#define magZmin -1335

//Defines for static offsets
#define heading_offset 0
#define heading_compensated_offset 0

//Calculate Exponential Moving Average
float calc_ema(float in, float average, float alpha);
float calc_ema(float in, float average, float alpha)
{

return ((alpha * in) + (1.0 - alpha) * average);
}

int main(int argc, char *argv[])

{

int magRaw[3];
int accRaw[3];
float scaledMag[3];

float accXnorm,accYnorm,pitch,roll,magXcomp,magYcomp;
float heading;
float heading2;
float average1 = 0;
float average2 = 0;
float average1disp = 0;
float average2disp = 0;
int32_t adc_value1;
int32_t adc_value2;

detectIMU();
enableIMU();

while(1)
{
readMAG(magRaw);
readACC(accRaw);

//Apply hard iron calibration
magRaw[0] -= (magXmin + magXmax) /2 ;
magRaw[1] -= (magYmin + magYmax) /2 ;
magRaw[2] -= (magZmin + magZmax) /2 ;

//Apply soft iron calibration
scaledMag[0] = (float)(magRaw[0] - magXmin) / (magXmax - magXmin) * 2 - 1;
scaledMag[1] = (float)(magRaw[1] - magYmin) / (magYmax - magYmin) * 2 - 1;
scaledMag[2] = (float)(magRaw[2] - magZmin) / (magZmax - magZmin) * 2 - 1;

//If your IMU is upside down(Skull logo facing up), comment out the two lines below which will correct the tilt calculation
//accRaw[0] = -accRaw[0];
//accRaw[1] = -accRaw[1];

//Compute heading

heading = 180 * atan2(scaledMag[1],scaledMag[0])/M_PI;
//float heading = 180 * atan2(magRaw[1],magRaw[0])/M_PI;
heading = heading - heading_offset;

adc_value1 = (float)heading;
average1 = calc_ema(adc_value1, average1, 0.1);

//Necessary with separate variable for display of the averaged heading as the function below creates problems with the averaging
average1disp = average1;

//Convert heading to 0 - 360
if(heading < 0)
heading += 360;

if(average1disp < 0)
average1disp += 360;

printf("Heading %7.3f \t ", heading);
printf("Averaged Heading %7.0f \t ", average1disp);

//Normalize accelerometer raw values.
accXnorm = accRaw[0]/sqrt(accRaw[0] * accRaw[0] + accRaw[1] * accRaw[1] + accRaw[2] * accRaw[2]);
accYnorm = accRaw[1]/sqrt(accRaw[0] * accRaw[0] + accRaw[1] * accRaw[1] + accRaw[2] * accRaw[2]);

//Calculate pitch and roll
pitch = asin(accXnorm);
roll = -asin(accYnorm/cos(pitch));

//Calculate the new tilt compensated values
magXcomp = magRaw[0]*cos(pitch)+magRaw[2]*sin(pitch);
if(LSM9DS0)
magYcomp = magRaw[0]*sin(roll)*sin(pitch)+magRaw[1]*cos(roll)-magRaw[2]*sin(roll)*cos(pitch); // LSM9DS0
else
magYcomp = magRaw[0]*sin(roll)*sin(pitch)+magRaw[1]*cos(roll)+magRaw[2]*sin(roll)*cos(pitch); // LSM9DS1

//Calculate heading
heading2 = (180*atan2(magYcomp,magXcomp)/M_PI)-heading_compensated_offset;

adc_value2 = (float)heading2;

average2 = calc_ema(adc_value2, average2, 0.1);
//Necessary with separate variable for display of the averaged heading as the function below creates problems with the averaging
average2disp = average2;

//Convert heading to 0 - 360
if(heading < 0)
heading += 360;

if(average2disp < 0)
average2disp += 360;

printf("Compensated Heading %7.3f\t", heading);

printf("Averaged Compensated Heading %7.0f\n", average2disp);

//Sleep for 25ms
usleep(25000);

}

}

 

 

[Author]

Posts