HMC5883L & ADXL345 with Adafruit Libraries (Tilt Compensation Issue)

dear friends,

i have built below codes tilt compensated compass (based on hmc5883l , adxl345) of adafruit libraries (see attached) :

code: [select]


// adafruit accel & compass 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 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 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 serial have strange output 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 tilt or roll module, heading changes seen above, on other side tilt compensated heading output changes lot instead of compensating

any ideas ?

it essential calibrate magnetometer. have done that?
i don't see accelerometer data used tilt compensation. might want take @ application note describing approach: http://cache.freescale.com/files/sensors/doc/app_note/an4248.pdf

Arduino Forum > Using Arduino > Sensors > HMC5883L & ADXL345 with Adafruit Libraries (Tilt Compensation Issue)

arduino