Important changes to forums and questions
All forums and questions are now archived. To start a new conversation or read the latest updates go to forums.mbed.com.
10 years, 1 month ago.
Compensating magnetic compass with accelerometer
Hi all,
I am trying to compensate a magnetic compass with an accelerometer (details below), all devices used have librarys for use on mbed. the
Details: •Mbed: LPC1768 •Compass: HMC5883L •Accelerometer: ADXL345
I am having a problem, I have followed the calculations in this: http://www.cypress.com/?docID=33225 But I dont get the right angle, 'heading' works when everything is on the horizontal but is wrong when tilted. 'Heading2' is the one where I am following the guide but i only get values between -2 and +2, not the heading.
double getHeading()
{
int16_t data[3];
int readings[3] = {0, 0, 0};
float XH, YH, H;
getData(data);
getCompensation(readings);
double X = static_cast<double>(data[0]);
double Y = static_cast<double>(data[2]);
double Z = static_cast<double>(data[1]);
double pitch = static_cast<double>(readings[1]);
double roll = static_cast<double>(readings[0]);
XH = X*cos((double) pitch) + Y*sin((double) roll)*sin((double) pitch) + Z*cos((double) roll)*sin((double) pitch);
YH = Y*cos((double) roll) + Z*sin((double) roll);
// XH = X + pitch;
// YH = Y - roll;
double Heading2;
if(XH < 0)
{
Heading2 = 180 - atan2(YH, XH);
}
else if(XH > 0 && YH < 0)
{
Heading2 = 0 - atan2(YH, XH);
}
else if(XH > 0 && YH > 0)
{
Heading2 = 360 - atan2(YH, XH);
}
else if(XH = 0 && YH < 0)
{
Heading2 = 90;
}
else if(XH = 0 && YH > 0)
{
Heading2 = 270;
}
else
{
Heading2 = 999999;
}
double heading = atan2(Y, X); // heading = arctan(Y/X)
if(heading < 0)
heading += 2*PI;
if(heading > 2*PI)
heading -= 2*PI;
heading = heading * 180 / PI;
pc.printf("Data X:%f, Y:%f, Z:%f, Pitch:%f, Roll:%f; \r\n", X, Y, Z, pitch, roll);
pc.printf("Compass1: %f \r\n", heading);
pc.printf("Compass2: %f \r\n", Heading2);
return Heading2;
}
I think the above code is correct and it might be an error in the setup code below. But I dont know.
//Go into standby mode to configure the device.
accelerometer.setPowerControl(0x00);
//Full resolution, +/-2g, 4mg/LSB.
accelerometer.setDataFormatControl(0x08);
//3.2kHz data rate.
accelerometer.setDataRate(ADXL345_3200HZ);
//Measurement mode.
accelerometer.setPowerControl(0x08);
void HMC5883L::init()
{
// init - configure your setup here
setConfigurationA(AVG8_SAMPLES | OUTPUT_RATE_15); // 8 sample average, 15Hz, normal mode
setConfigurationB(0x20); // default
setMode(CONTINUOUS_MODE); // continuous sample mode
}
Can anyone see an error or is able to help me?
Regards, Nick
1 Answer
10 years, 1 month ago.
If it is an error in your setup code you should see it in the raw data. X-Y-Z data doesn't matter as long as you didn't mix them up, scale factors are irrelevant there. Pitch and roll start as int's, which you cast to doubles (might be overkill, but okay). But what are they? Degrees or radians? If degrees it could be correct they start as ints, but a radian (so between 0 and 2*pi) cannot be an int if you want to have some accuracy.
Then in your next code you are calculating heading-2 offsets depending on quadrant it is in using degrees. But standard C(++) math should always return radians, not degrees. (so 180 degrees is Pi, 90 is half Pi, etc).
Finally heading you do compensate for radians/degrees: You calculate first with radians, and then scale it to degrees. If you want your output in degrees you should do the same with heading2, mulitply by 180, divide by Pi.