Dear Friends,
I have built below codes for Tilt Compensated Compass (based on HMC5883L and ADXL345) with help of Adafruit Libraries (see attached) :
// Adafruit Accel & Compass for Tilt Compensated
#include "Wire.h"
#include "Adafruit_Sensor.h"
#include "Adafruit_ADXL345_U.h"
#include "Adafruit_HMC5883_U.h"
Adafruit_ADXL345_Unified accel = Adafruit_ADXL345_Unified(12345);
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
float magHeadingX, magHeadingY;
float magHeadingAbsolute = 0.0;
void magnetometer(void) {
sensors_event_t event;
mag.getEvent(&event);
sensor_t sensor;
mag.getSensor(&sensor);
Serial.print("MX: "); Serial.print(event.magnetic.x); Serial.print(" ");
Serial.print("MY: "); Serial.print(event.magnetic.y); Serial.print(" ");
Serial.print("MZ: "); Serial.print(event.magnetic.z); Serial.print(" ");Serial.println("uT");
delay(1000); }
void accelerometer(void) {
sensors_event_t event;
accel.getEvent(&event);
sensor_t sensor;
accel.getSensor(&sensor);
Serial.print("AX: "); Serial.print(event.acceleration.x); Serial.print(" ");
Serial.print("AY: "); Serial.print(event.acceleration.y); Serial.print(" ");
Serial.print("AZ: "); Serial.print(event.acceleration.z); Serial.print(" ");Serial.println("m/s^2 ");
delay(1000); }
void heading(void) {
sensors_event_t event;
mag.getEvent(&event);
sensor_t sensor;
mag.getSensor(&sensor);
float heading = atan2(event.magnetic.y, event.magnetic.x);
float declinationAngle = 0.0766;
heading -= declinationAngle;
if(heading < 0)
heading += 2*PI;
if(heading > 2*PI)
heading -= 2*PI;
float headingDegrees = heading * 180/M_PI; // Radians to Degrees
Serial.print("Heading (degrees): "); Serial.println(headingDegrees);
delay(1000); }
void tcheading(void) {
sensors_event_t event;
mag.getEvent(&event);
sensor_t sensor;
mag.getSensor(&sensor);
float cosRoll = cos(event.magnetic.x);
float sinRoll = sin(event.magnetic.x);
float cosPitch = cos(event.magnetic.y);
float sinPitch = sin(event.magnetic.y);
float magX, magY;
magX = event.magnetic.x * cosPitch * event.magnetic.z * sinPitch;
magY = event.magnetic.x * sinRoll * sinPitch + event.magnetic.y * cosRoll - event.magnetic.z * sinRoll * cosPitch;
float norm = sqrt(magX * magX + magY * magY);
magHeadingX = magX / norm;
magHeadingY = -magY / norm;
magHeadingAbsolute = atan2(magHeadingY, magHeadingX);
float declinationAngle = 0.0766;
magHeadingAbsolute -= declinationAngle;
if(magHeadingAbsolute < 0)
magHeadingAbsolute += 2*PI;
if(magHeadingAbsolute > 2*PI)
magHeadingAbsolute -= 2*PI;
float headingDegrees = magHeadingAbsolute * 180/M_PI; // Radians to Degrees
Serial.print("Compensated Heading (degrees): "); Serial.println(headingDegrees);
delay(1000); }
void setup(void) {
Serial.begin(9600);
accel.begin();
accel.setRange(ADXL345_RANGE_2_G);
mag.begin();
mag.setMagGain(HMC5883_MAGGAIN_1_3);
}
void loop(void) {
magnetometer();
accelerometer();
heading();
tcheading();
}
The result from serial have strange output as below :
...
MX: 1.91 MY: -3.64 MZ: 63.88 uT
AX: 4.59 AY: 0.51 AZ: -9.45 m/s^2
Heading (degrees): 282.31
Compensated Heading (degrees): 356.49
MX: 19.09 MY: -5.18 MZ: 59.29 uT
AX: 0.31 AY: 0.39 AZ: -10.24 m/s^2
Heading (degrees): 341.51
Compensated Heading (degrees): 325.76
MX: 19.73 MY: -4.73 MZ: 59.08 uT
AX: 0.27 AY: 0.35 AZ: -10.28 m/s^2
Heading (degrees): 341.89
Compensated Heading (degrees): 352.25
MX: 19.64 MY: -4.82 MZ: 59.08 uT
AX: 0.24 AY: 0.35 AZ: -10.28 m/s^2
Heading (degrees): 342.01
Compensated Heading (degrees): 226.55
MX: 7.00 MY: -2.45 MZ: 63.16 uT
AX: 3.45 AY: 0.51 AZ: -9.77 m/s^2
Heading (degrees): 324.49
Compensated Heading (degrees): 22.14
....
When i tilt or roll the module, the Heading changes as seen above, but on the other side Tilt Compensated Heading output changes a lot instead of compensating
Any i ideas ?
Adafruit Libraries.zip (22.7 KB)