LSM303DLHC eCompass implementation

eCompass.ino

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include <orientation.h>

// For the fabs function in fixedWidthPrint
#include <Math.h>

/* Assign a unique ID to this sensor at the same time */
Adafruit_LSM303_Accel_Unified accl = Adafruit_LSM303_Accel_Unified(54321);
Adafruit_LSM303_Mag_Unified magn = Adafruit_LSM303_Mag_Unified(12345);

void displaySensorDetails(void)
{
  sensor_t sensor;
  accl.getSensor(&sensor);
  Serial.println("------------------------------------");
  Serial.print  ("Sensor:       "); 
  Serial.println(sensor.name);
  Serial.print  ("Driver Ver:   "); 
  Serial.println(sensor.version);
  Serial.print  ("Unique ID:    "); 
  Serial.println(sensor.sensor_id);
  Serial.print  ("Max Value:    "); 
  Serial.print(sensor.max_value); 
  Serial.println(" m/s^2");
  Serial.print  ("Min Value:    "); 
  Serial.print(sensor.min_value); 
  Serial.println(" m/s^2");
  Serial.print  ("Resolution:   "); 
  Serial.print(sensor.resolution); 
  Serial.println(" m/s^2");  
  Serial.println("------------------------------------");
  Serial.println("");
  delay(500);
}

void setup(void) 
{
  Serial.begin(9600);
  Serial.println("Tilt Compensated Compass  Test"); 
  Serial.println("");

  /* Initialise the sensor */
  if(!accl.begin() || !magn.begin())  // also initalizes mag and accel sensors
  {
    /* There was a problem detecting the ADXL345 ... check your connections */
    Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
    while(1);
  }

  /* Enable auto-gain */
  magn.enableAutoRange(true);
  /* Display some basic information on this sensor */
  displaySensorDetails();
  
  
  // Boston geomagnetic field
  // Declination -14 degrees
  // Inclunation -6.4 degrees
  // Horizontal Intensity 20,000 nT
  // North Component 19,000 nT north
  // East Component 5,000 nT west
  // Vertical Component 48,000 nT down
  // Total Field 52,000 nT
  float declination = -14;

  // Boat  
  float hardiron_x = -9.715;
  float hardiron_y = 2.77;
  float hardiron_z = -2.955;
  
  Orientation::setParameters(&accl, &magn, declination, hardiron_x, hardiron_y, hardiron_z);
}

void fixedWidthPrint(float value){
  // Bug: one too many spaces when value === 10
  int characters = 0;
   
  // Count the number of digits before the decimal point
  for(int i = 10000; i > 0; i /= 10){
     if((int)fabs(value) >= i){
       characters ++;
     }
  } 
  
  if(characters == 0){
    characters = 1; // Minimum of 1 character if we print '0'
  }
  
  if(fabs(value) != value){ // Is it negative?
    characters++;
  }
  
  for(int i = 6; i > characters; i--){
    Serial.print(' ');
  }
  Serial.print(value, 2);
}

void loop(void) 
{ 
  float roll;
  float pitch;
  float yaw;
  float heading;

  Orientation::calculate(roll, pitch, yaw, heading);
 
  Serial.print(" | roll:"); fixedWidthPrint(roll);
  Serial.print(" | pitch:"); fixedWidthPrint(pitch);  
  Serial.print(" | yaw:"); fixedWidthPrint(yaw);
  Serial.print(" | heading:"); fixedWidthPrint(heading);
  
  Serial.print('\n');
}

graph_magn.gpi

set term png
set output "magn.png"
set terminal png size 1600,1600

print "##### magn x"
stats 'magn.tsv' using 1
print "##### magn y"
stats 'magn.tsv' using 2
print "##### magn z"
stats 'magn.tsv' using 3

set multiplot layout 2, 2

set xlabel "x"
set ylabel "y"
set size square
plot 'magn.tsv' using 1:2 with points title "x vs. y"
set nolabel

set xlabel "x"
set ylabel "z"
plot 'magn.tsv' using 1:3 with points title "x vs. z"

set xlabel "y"
set ylabel "z"
plot 'magn.tsv' using 2:3 with points title "y vs. z"

# 3D Plot
set nokey
set xlabel "x"
set ylabel "y"
set zlabel "z"
set view equal xyz
splot 'magn.tsv' with points

unset multiplot

hardironcalibration.ino

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>

/* Assign a unique ID to these sensors */
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(54321);
Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(12345);

float AccelMinX, AccelMaxX;
float AccelMinY, AccelMaxY;
float AccelMinZ, AccelMaxZ;

float MagMinX, MagMaxX;
float MagMinY, MagMaxY;
float MagMinZ, MagMaxZ;

long lastDisplayTime;

void setup(void) 
{
  Serial.begin(9600);
  Serial.println("LSM303 Calibration"); Serial.println("");
  
  /* Initialise the accelerometer */
  if(!accel.begin())
  {
    /* There was a problem detecting the ADXL345 ... check your connections */
    Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
    while(1);
  }
  /* Initialise the magnetometer */
  if(!mag.begin())
  {
    /* There was a problem detecting the LSM303 ... check your connections */
    Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
    while(1);
  }
  lastDisplayTime = millis();
}

void loop(void) 
{
  /* Get a new sensor event */ 
  sensors_event_t accelEvent; 
  sensors_event_t magEvent; 
  
  accel.getEvent(&accelEvent);
  mag.getEvent(&magEvent);
  
  if (accelEvent.acceleration.x < AccelMinX) AccelMinX = accelEvent.acceleration.x;
  if (accelEvent.acceleration.x > AccelMaxX) AccelMaxX = accelEvent.acceleration.x;
  
  if (accelEvent.acceleration.y < AccelMinY) AccelMinY = accelEvent.acceleration.y;
  if (accelEvent.acceleration.y > AccelMaxY) AccelMaxY = accelEvent.acceleration.y;

  if (accelEvent.acceleration.z < AccelMinZ) AccelMinZ = accelEvent.acceleration.z;
  if (accelEvent.acceleration.z > AccelMaxZ) AccelMaxZ = accelEvent.acceleration.z;

  if (magEvent.magnetic.x < MagMinX) MagMinX = magEvent.magnetic.x;
  if (magEvent.magnetic.x > MagMaxX) MagMaxX = magEvent.magnetic.x;
  
  if (magEvent.magnetic.y < MagMinY) MagMinY = magEvent.magnetic.y;
  if (magEvent.magnetic.y > MagMaxY) MagMaxY = magEvent.magnetic.y;

  if (magEvent.magnetic.z < MagMinZ) MagMinZ = magEvent.magnetic.z;
  if (magEvent.magnetic.z > MagMaxZ) MagMaxZ = magEvent.magnetic.z;

  if ((millis() - lastDisplayTime) > 1000)  // display once/second
  {
    float hardiron_x = (MagMaxX + MagMinX) / 2;
    float hardiron_y = (MagMaxY + MagMinY) / 2;
    float hardiron_z = (MagMaxZ + MagMinZ) / 2;
    
    Serial.print(" | hardiron_x: "); Serial.print(hardiron_x, 3); Serial.print(" | hardiron_y: "); Serial.print(hardiron_y, 3); Serial.print(" | hardiron_z:"); Serial.print(hardiron_z, 3);
    Serial.print('\n');
    lastDisplayTime = millis();
  }
}

magnprint.ino

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>

/* Assign a unique ID to these sensors */
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(54321);
Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(12345);

float AccelMinX, AccelMaxX;
float AccelMinY, AccelMaxY;
float AccelMinZ, AccelMaxZ;

float MagMinX, MagMaxX;
float MagMinY, MagMaxY;
float MagMinZ, MagMaxZ;

long lastDisplayTime;

void setup(void) 
{
  Serial.begin(9600);
  
  /* Initialise the accelerometer */
  if(!accel.begin())
  {
    /* There was a problem detecting the ADXL345 ... check your connections */
    Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
    while(1);
  }
  /* Initialise the magnetometer */
  if(!mag.begin())
  {
    /* There was a problem detecting the LSM303 ... check your connections */
    Serial.println("Ooops, no LSM303 detected ... Check your wiring!");
    while(1);
  }
  lastDisplayTime = millis();
}

void loop(void) 
{
  /* Get a new sensor event */ 
  sensors_event_t accelEvent; 
  sensors_event_t magEvent; 
  
  accel.getEvent(&accelEvent);
  mag.getEvent(&magEvent);
  
  float accl_x = accelEvent.acceleration.x;
  float accl_y = accelEvent.acceleration.y;
  float accl_z = accelEvent.acceleration.z;
  
  float magn_x = magEvent.magnetic.x;
  float magn_y = magEvent.magnetic.y;
  float magn_z = magEvent.magnetic.z;
  
  Serial.print(magn_x);
  Serial.print("\t");
  Serial.print(magn_y);
  Serial.print("\t");
  Serial.print(magn_z);
  Serial.print("\n");
  
  delay(10); // Limit to, at most, 100Hz.
}

orientation.cpp

#include <orientation.h>

#define RAD_CONV 57.2957 // 180/Pi, to convert radians to degrees

float Orientation::hardiron_x, Orientation::hardiron_y, Orientation::hardiron_z;
float Orientation::declination;
Adafruit_LSM303_Accel_Unified * Orientation::accl;
Adafruit_LSM303_Mag_Unified * Orientation::magn;

/** Use an eCompass algorithm to calculate orientation
 * 
 * The algorithm is based on http://cache.freescale.com/files/sensors/doc/app_note/AN4248.pdf
 * 
 * Hardiron calibration must be performed. The process is simple:
 *   1. Mount the magnetometer in the location that you intend to use it at
 *   2. Rotate the body through all possible orientations
 *   3. Record the minimum and maximum for each axis of the magnetometer
 *   4. Average the minumum and maximum for each axis. This will give you your hardiron x,y,z offsets.
 *
 * @param accl The LSM303 acceleration object
 * @param magn The LSM303 magnetometer object
 * @param hardiron_x
 * @param hardiron_y
 * @param hardiron_z
 * @param roll The result roll in degrees
 * @param pitch The result pitch in degrees
 * @param yaw The result yaw in degrees
 */
void Orientation::calculate(float & roll, float & pitch, float & yaw, float & heading){
  
  // Get a new sensor event
  sensors_event_t event_accl; 
  sensors_event_t event_magn; 

  accl->getEvent(&event_accl); 
  magn->getEvent(&event_magn);

  // Signs choosen so that, when axis is down, the value is + 1g
  float accl_x = -event_accl.acceleration.x;
  float accl_y = event_accl.acceleration.y;
  float accl_z = event_accl.acceleration.z;

  // Signs should be choosen so that, when the axis is down, the value is + positive.
  // But that doesn't seem to work ?...
  float magn_x = event_magn.magnetic.x - hardiron_x;
  float magn_y = -event_magn.magnetic.y - hardiron_y;
  float magn_z = -event_magn.magnetic.z - hardiron_z;  
  
  // Freescale solution
  roll = atan2(accl_y, accl_z);
  pitch = atan(-accl_x / (accl_y * sin(roll) + accl_z * cos(roll)));
  
  float magn_fy_fs = magn_z * sin(roll) - magn_y*cos(roll);
  float magn_fx_fs = magn_x * cos(pitch) + magn_y * sin(pitch) * sin(roll) + magn_z * sin(pitch) * cos(roll);
  
  yaw = atan2(magn_fy_fs, magn_fx_fs);
  
  roll = roll * RAD_CONV;
  pitch = pitch * RAD_CONV;
  yaw = yaw * RAD_CONV;
  
  heading = yawToHeading(yaw);
}

/** Convert a yaw (-180 to 180) to a compass heading (0 to 360) with declination correction
 * 
 * @param yaw The input yaw, in degrees -180 to 180
 * @param declination the magnetic declination to adjust against, with sign. Note that this is time and location dependent.
 * @return the corrected heading, in degrees 0 to 360
 */
float Orientation::yawToHeading(float yaw){
    float heading = yaw + declination;
    if (heading < 0.0){
        heading += 360.0;
    }
    return heading;
}


void Orientation::setParameters(
    Adafruit_LSM303_Accel_Unified * new_accl, Adafruit_LSM303_Mag_Unified * new_magn,
    float new_declination,
    float new_hardiron_x, float new_hardiron_y, float new_hardiron_z){
    accl = new_accl;
    magn = new_magn;
    declination = new_declination;
    hardiron_x = new_hardiron_x;
    hardiron_y = new_hardiron_y;
    hardiron_z = new_hardiron_z;
}

orientation.h

#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>

class Orientation {
 public:
  static void calculate(float & roll, float & pitch, float & yaw, float & heading);
  static float yawToHeading(float yaw);

  static void setParameters(
            Adafruit_LSM303_Accel_Unified * accl, Adafruit_LSM303_Mag_Unified * magn,
            float declination,
            float hardiron_x, float hardiron_y, float hardiron_z);

  static Adafruit_LSM303_Accel_Unified * accl;
  static Adafruit_LSM303_Mag_Unified * magn;
  static float declination;
  static float hardiron_x, hardiron_y, hardiron_z;
};

stats.cpp

#include <iostream>
#include <fstream>

using namespace::std;

void compare(float value, float & min, float & max){
    if(value < min){
        min = value;
    }
    if(value > max){
        max = value;
    }
}

void printResult(string axis, float min, float max){
    cout << axis
         << ": min: " << min
         << ", max: " << max 
         << ", range: " << (max - min) 
         << ", middle: " << ((max - min)/2 + min)
         << "\n";
}

int main()
{
    cout << "Hello, world!\n";
    ifstream infile("magn.tsv");

    float x, y, z;

    float min_x, max_x;
    float min_y, max_y;
    float min_z, max_z;
    
    int line = 0;
    while(infile >> x >> y >> z){
        line ++;
        if(z < -1000){
            cout << "line: " << line << "\n";
        }

        compare(x, min_x, max_x);
        compare(y, min_y, max_y);
        compare(z, min_z, max_z);
    }

    printResult("X", min_x, max_x);
    printResult("Y", min_y, max_y);
    printResult("Z", min_z, max_z);
}