Skip to content

Instantly share code, notes, and snippets.

@vbahtev
Created August 10, 2018 05:07
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
Star You must be signed in to star a gist
Save vbahtev/b05f1895776cec5828db6746d5ba9a11 to your computer and use it in GitHub Desktop.
BerryIMU v1 and v2: Correct heading after hard iron calibration
/*
This program reads the angles and heading from the accelerometer, gyroscope
and compass on a BerryIMU connected to an Arduino. Make 360 degrees rotation
for apply hard iron calibration.
Both the BerryIMUv1 and BerryIMUv2 are supported.
Feel free to do whatever you like with this code
Distributed as-is; no warranty is given.
http://ozzmaker.com/
*/
#include "IMU.h"
#define DT 0.02 // Loop time
#define AA 0.97 // complementary filter constant
#define G_GAIN 0.070 // [deg/s/LSB]
byte buff[6];
int accRaw[3];
int magRaw[3];
int gyrRaw[3];
float rate_gyr_y = 0.0; // [deg/s]
float rate_gyr_x = 0.0; // [deg/s]
float rate_gyr_z = 0.0; // [deg/s]
float gyroXangle = 0.0;
float gyroYangle = 0.0;
float gyroZangle = 0.0;
float AccYangle = 0.0;
float AccXangle = 0.0;
float CFangleX = 0.0;
float CFangleY = 0.0;
// used for hard iron calibration
int magXmax = -32767;
int magYmax = -32767;
int magZmax = -32767;
int magXmin = 32767;
int magYmin = 32767;
int magZmin = 32767;
unsigned long startTime;
void setup() {
// join i2c bus (address optional for master)
Serial.begin(115200); // start serial for output
detectIMU();
enableIMU();
}
void loop() {
startTime = millis();
//Read the measurements from sensors
readACC(buff);
accRaw[0] = (int)(buff[0] | (buff[1] << 8));
accRaw[1] = (int)(buff[2] | (buff[3] << 8));
accRaw[2] = (int)(buff[4] | (buff[5] << 8));
readMAG(buff);
magRaw[0] = (int)(buff[0] | (buff[1] << 8));
magRaw[1] = (int)(buff[2] | (buff[3] << 8));
magRaw[2] = (int)(buff[4] | (buff[5] << 8));
readGYR(buff);
gyrRaw[0] = (int)(buff[0] | (buff[1] << 8));
gyrRaw[1] = (int)(buff[2] | (buff[3] << 8));
gyrRaw[2] = (int)(buff[4] | (buff[5] << 8));
//Convert Gyro raw to degrees per second
rate_gyr_x = (float) gyrRaw[0] * G_GAIN;
rate_gyr_y = (float) gyrRaw[1] * G_GAIN;
rate_gyr_z = (float) gyrRaw[2] * G_GAIN;
//Calculate the angles from the gyro
gyroXangle+=rate_gyr_x*DT;
gyroYangle+=rate_gyr_y*DT;
gyroZangle+=rate_gyr_z*DT;
//Convert Accelerometer values to degrees
AccXangle = (float) (atan2(accRaw[1],accRaw[2])+M_PI)*RAD_TO_DEG;
AccYangle = (float) (atan2(accRaw[2],accRaw[0])+M_PI)*RAD_TO_DEG;
//If IMU is up the correct way, use these lines
AccXangle -= (float)180.0;
if (AccYangle > 90)
AccYangle -= (float)270;
else
AccYangle += (float)90;
//Complementary filter used to combine the accelerometer and gyro values.
CFangleX=AA*(CFangleX+rate_gyr_x*DT) +(1 - AA) * AccXangle;
CFangleY=AA*(CFangleY+rate_gyr_y*DT) +(1 - AA) * AccYangle;
if (magRaw[0] > magXmax) magXmax = magRaw[0];
if (magRaw[1] > magYmax) magYmax = magRaw[1];
if (magRaw[2] > magZmax) magZmax = magRaw[2];
if (magRaw[0] < magXmin) magXmin = magRaw[0];
if (magRaw[1] < magYmin) magYmin = magRaw[1];
if (magRaw[2] < magZmin) magZmin = magRaw[2];
//Apply hard iron calibration
magRaw[0] = magRaw[0] - (magXmin + magXmax) /2 ;
magRaw[1] = magRaw[1] - (magYmin + magYmax) /2 ;
magRaw[2] = magRaw[2] - (magZmin + magZmax) /2 ;
//Compute heading
float heading = 180 * atan2(magRaw[1],magRaw[0])/M_PI;
//Convert heading to 0 - 360
if(heading < 0)
heading += 360;
Serial.print("#AccX\t");
Serial.print(AccXangle);
Serial.print("\t### AccY ");
Serial.print(AccYangle);
Serial.print(" ### GyrX\t");
Serial.print(gyroXangle);
Serial.print(" ### GyrY \t");
Serial.print(gyroYangle);
Serial.print(" ### GyrZ\t");
Serial.print(gyroZangle);
Serial.print(" ###### CFangleX\t");
Serial.print(CFangleX);
Serial.print(" ###### CFangleY \t");
Serial.print(CFangleY);
Serial.print(" ###### heading \t");
Serial.print(heading);
Serial.print(" --Loop Time--\t");
//Each loop should be at least 20ms.
while(millis() - startTime < (DT*1000))
{
delay(1);
}
Serial.println( millis()- startTime);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment