The HMC5883L data acquisition I wrote :
// i just need a direction reference not actual heading, just to keep my drone
// steady on yaw axis with reference to that direction reference but it shall be Tilt compensated
#include<Wire.h>
#define MAG 0x1E
#define R 0
#define P 1
#define Y 2
float mag_raw[3],h;
float CMP_DEC = 0.36667 * (PI/180);
void setup(){
Serial.begin(115200);
Wire.begin();
Wire.setClock(400000);
Wire.beginTransmission(MAG);
Wire.write(0x02);
Wire.write(0x00);
Wire.endTransmission(true);
}
void loop(){
Wire.beginTransmission(MAG);
Wire.write(0x03);
Wire.endTransmission(false);
Wire.requestFrom(MAG,6,true);
mag_raw[R] = Wire.read() << 8 | Wire.read();
mag_raw[P] = Wire.read() << 8 | Wire.read();
mag_raw[Y] = Wire.read() << 8 | Wire.read();
h = atan2(mag_raw[P],mag_raw[R]);
// h += CMP_DEC;
if(h < 0) h+= (2*PI);
if(h > (2*PI)) h -= (2*PI);
h *= (180/PI);
// Serial.print("X : ");
// Serial.print(mag_raw[R]);
// Serial.print(" Y : ");
// Serial.print(mag_raw[P]);
// Serial.print(" Z : ");
// Serial.print(mag_raw[Y]);
Serial.print(" H : ");
Serial.println(h);
}
Some guidance will be very helpful!
Some guidance will be very helpful!
The HMC5883L data acquisition I wrote :
// i just need a direction reference not actual heading, just to keep my drone
// steady on yaw axis with reference to that direction reference but it shall be Tilt compensated
#include<Wire.h>
#define MAG 0x1E
#define R 0
#define P 1
#define Y 2
float mag_raw[3],h;
float CMP_DEC = 0.36667 * (PI/180);
void setup(){
Serial.begin(115200);
Wire.begin();
Wire.setClock(400000);
Wire.beginTransmission(MAG);
Wire.write(0x02);
Wire.write(0x00);
Wire.endTransmission(true);
}
void loop(){
Wire.beginTransmission(MAG);
Wire.write(0x03);
Wire.endTransmission(false);
Wire.requestFrom(MAG,6,true);
mag_raw[R] = Wire.read() << 8 | Wire.read();
mag_raw[P] = Wire.read() << 8 | Wire.read();
mag_raw[Y] = Wire.read() << 8 | Wire.read();
h = atan2(mag_raw[P],mag_raw[R]);
// h += CMP_DEC;
if(h < 0) h+= (2*PI);
if(h > (2*PI)) h -= (2*PI);
h *= (180/PI);
// Serial.print("X : ");
// Serial.print(mag_raw[R]);
// Serial.print(" Y : ");
// Serial.print(mag_raw[P]);
// Serial.print(" Z : ");
// Serial.print(mag_raw[Y]);
Serial.print(" H : ");
Serial.println(h);
}
Some guidance will be very helpful!
How to implement a tilt compensated compass?
So i was trying to implement a tilt compensated compass, using HMC5883L and MPU6050. The problem i am facing is, there are 2 equations for tilt compensation
Tilt Compensated X: X,Y,Z are compass output (raw output)
new_X = X*cos(pitch) + Y*sin(roll)*sin(pitch) – Z*cos(roll)*sin(pitch)
Tilt Compensated Y:
new Y = Y*cos(roll) + Z*sin(roll)
Final Heading :
Azimuth = atan2(Y, X) * RAD_TO_DEG
The angles which i am getting from the mpu are gyro-drift corrected angles which are coming through a kalman filter. Those angles are fed to these above equations and after atan2 i get the tilt compensated heading. But the issue/observation here is, when i rotate the body(on which the compass and mpu are) along the Z axis (YAW axis) the heading doesn't change(sort of locked on), but changes when i tilt the body in X/Y.
I searched for thorough references, but wasn't able to find any!
Here is my code snippet (uses compass part form adafruit library):
double HEADING(double r, double p){
sensors_event_t event;
mag.getEvent(&event);
r = r * (PI/180);
p = p * (PI/180);
double x = (event.magnetic.x)*cos(p) + (event.magnetic.y)*sin(r)*sin(p) - (event.magnetic.z)*cos(r)*sin(p);
double y = (event.magnetic.y)*cos(r) + (event.magnetic.z)*sin(r);
float heading = atan2(y,x);
float declinationAngle = 0.366667 * (PI/180);// (0°22')positve declination
heading += declinationAngle;
if(heading < 0)
heading += 2*PI;
if(heading > 2*PI)
heading -= 2*PI;
heading = heading * 180/PI;
return heading;
}
Some guidance will be very helpful!