I am using an HMC-5983 magnetometer compass on my arduino uno to find the heading of a car I'm building.But my compass readings aren't varying linearly.For example when I physically turn my compass by 90 degrees the change in the reading doesn't correspond to 90 degrees.I tried mapping the raw output of the compass to the actual heading but when I do this , the headings in between the points I mapped aren't accurate enough thereby causing my car to behave abnormally.I also looked up on google but couldn't find anything helpful though there was some mention about using an algorithm to sort this issue.Please help me fix this problem.
#include <Wire.h> //I2C Arduino Library
#define Magnetometer_mX0 0x03
#define Magnetometer_mX1 0x04
#define Magnetometer_mZ0 0x05
#define Magnetometer_mZ1 0x06
#define Magnetometer_mY0 0x07
#define Magnetometer_mY1 0x08
int mX0, mX1, mX_out;
int mY0, mY1, mY_out;
int mZ0, mZ1, mZ_out;
float heading, headingDegrees, headingFiltered, declination;
float Xm,Ym,Zm;
#define Magnetometer 0x1E //I2C 7bit address of HMC5883
void setup(){
//Initialize Serial and I2C communications
Serial.begin(9600);
Wire.begin();
delay(100);
Wire.beginTransmission(Magnetometer);
Wire.write(0x02); // Select mode register
Wire.write(0x00); // Continuous measurement mode
Wire.endTransmission();
}
void loop(){
//---- X-Axis
Wire.beginTransmission(Magnetometer); // transmit to device
Wire.write(Magnetometer_mX1);
Wire.endTransmission();
Wire.requestFrom(Magnetometer,1);
if(Wire.available()<=1)
{
mX0 = Wire.read();
}
Wire.beginTransmission(Magnetometer); // transmit to device
Wire.write(Magnetometer_mX0);
Wire.endTransmission();
Wire.requestFrom(Magnetometer,1);
if(Wire.available()<=1)
{
mX1 = Wire.read();
}
//---- Y-Axis
Wire.beginTransmission(Magnetometer); // transmit to device
Wire.write(Magnetometer_mY1);
Wire.endTransmission();
Wire.requestFrom(Magnetometer,1);
if(Wire.available()<=1)
{
mY0 = Wire.read();
}
Wire.beginTransmission(Magnetometer); // transmit to device
Wire.write(Magnetometer_mY0);
Wire.endTransmission();
Wire.requestFrom(Magnetometer,1);
if(Wire.available()<=1)
{
mY1 = Wire.read();
}
//---- Z-Axis
Wire.beginTransmission(Magnetometer); // transmit to device
Wire.write(Magnetometer_mZ1);
Wire.endTransmission();
Wire.requestFrom(Magnetometer,1);
if(Wire.available()<=1)
{
mZ0 = Wire.read();
}
Wire.beginTransmission(Magnetometer); // transmit to device
Wire.write(Magnetometer_mZ0);
Wire.endTransmission();
Wire.requestFrom(Magnetometer,1);
if(Wire.available()<=1)
{
mZ1 = Wire.read();
}
//---- X-Axis
mX1=mX1<<8;
mX_out =mX0+mX1; // Raw data
// From the datasheet: 0.92 mG/digit
Xm = mX_out*0.00092; // Gauss unit
//* Earth magnetic field ranges from 0.25 to 0.65 Gauss, so these are the values that we need to get approximately.
//---- Y-Axis
mY1=mY1<<8;
mY_out =mY0+mY1;
Ym = mY_out*0.00092;
//---- Z-Axis
mZ1=mZ1<<8;
mZ_out =mZ0+mZ1;
Zm = mZ_out*0.00092;
// ==============================
//Calculating Heading
heading = filtered_angle(Zm, Xm);// arc tangent of z/x
// Correcting the heading with the declination angle depending on your location
// You can find your declination angle at: http://www.ngdc.noaa.gov/geomag-web/
// At my location it's 4.2 degrees => 0.073 rad
declination = 0.03717551307 ;
heading += declination;
// Correcting when signs are reveresed
if(heading <0) heading += 2*PI;
// Correcting due to the addition of the declination angle
if(heading > 2*PI)heading -= 2*PI;
headingDegrees = heading * 180/PI; // The heading in Degrees unit
// Smoothing the output angle / Low pass filter
headingFiltered = headingDegrees;
//Sending the heading value through the Serial Port to Processing IDE
if(headingFiltered >= 0 && headingFiltered <= 89){
c_head=map(headingFiltered,0,90,270,359);
}
if(headingFiltered > 89 && headingFiltered <= 204){
c_head=map(headingFiltered,90,204,0,90);
}
if(headingFiltered > 204 && headingFiltered <= 290){
c_head=map(headingFiltered,205,290,91,180);
}
if(headingFiltered > 290){
c_head=map(headingFiltered,291,359,181,269);
}
// Serial.print("Degrees - ");
Serial.println(headingDegrees);
delay(500);
}
float filtered_angle(float p, float q) {
const float filter_constant = 0.15;
static float q_f, p_f; // filtered components
q_f += filter_constant * (q - q_f);
p_f += filter_constant * (p - p_f);
return atan2(p, q);
}
map()
) transformation in your code. Why are you doing that? Why do you expect your non-linear transformation to yield a linear result? – Edgar Bonet Nov 30 '19 at 17:39