HMC5883L Magnetometer Code

HMC5883L.cpp

#include "compass.h"

// COMPASS_MULTIPLIERS are calibrated per sensor and require an rigorous
// test environment. See README.md for more information.
int16_t COMPASS_MULTIPLIERS[360] = { 
  -213, -215, -213, -213, -213, -211, -208, -208, -204, -208, -206, -204, -204, -202, -200, -200, -200, -200, -202, -196, -196, -196, -196, -196, -196, -196, -192, -196, -192, -192, -192, -192, -192, -190, -189, -189, -189, -189, -189, -189, -189, -189, -189, -189, -187, -188, -185, -189, -185, -185, -185, -185, -185, -185, -185, -185, -185, -185, -185, -185, -185, -185, -185, -185, -185, -185, -185, -185, -185, -185, -185, -189, -185, -185, -185, -185, -185, -185, -185, -185, -189, -189, -187, -189, -185, -189, -189, -189, -189, -189, -192, -189, -190, -190, -190, -192, -192, -192, -192, -192, -196, -192, -196, -196, -196, -196, -196, -196, -196, -196, -200, -200, -200, -196, -200, -200, -200, -201, -204, -204, -200, -204, -204, -204, -204, -204, -204, -208, -208, -208, -208, -213, -213, -208, -208, -208, -208, -213, -213, -213, -213, -213, -217, -217, -217, -217, -217, -217, -217, -222, -222, -222, -222, -222, -222, -222, -222, -222, -222, -222, -227, -227, -227, -227, -227, -227, -227, -227, -227, -227, -227, -227, -227, -233, -233, -238, -238, -238, -233, -238, -238, -238, -238, -238, -244, -244, -238, -238, -244, -244, -244, -238, -238, -238, -244, -244, -244, -244, -244, -246, -246, -250, -250, -247, -247, -244, -250, -250, -250, -250, -250, -250, -253, -253, -250, -250, -250, -256, -256, -256, -256, -256, -256, -256, -256, -256, -256, -256, -263, -256, -263, -260, -263, -263, -256, -260, -263, -263, -260, -260, -263, -263, -263, -263, -263, -270, -270, -263, -263, -263, -270, -270, -270, -270, -270, -270, -270, -270, -270, -284, -284, -286, -284, -286, -283, -286, -281, -286, -283, -286, -280, -278, -275, -275, -275, -270, -273, -273, -274, -278, -274, -274, -270, -278, -274, -278, -278, -275, -278, -270, -270, -274, -274, -273, -270, -272, -270, -270, -272, -270, -270, -274, -274, -270, -270, -268, -263, -270, -266, -270, -270, -270, -263, -263, -263, -263, -262, -268, -268, -263, -256, -256, -256, -256, -253, -253, -250, -250, -250, -250, -244, -244, -244, -244, -244, -238, -238, -238, -244, -238, -233, -233, -235, -233, -233, -233, -230, -227, -230, -227, -227, -225, -227, -225, -222, -222, -222, -220, -217, -217
};

// Given a particular gain (0 - 7) return the value which, if configured, will yield that gain setting
int COMPASS_GAINS[8] = {0x00, 0x20, 0x40, 0x60, 0x80, 0xA0, 0xC0, 0xE0};

// Given a particular gain, define the lower and upper limit of what constitutes a
// a "positive self test" value.
int GAIN_LIMITS[8][2] = {
  {853, 2019}, // Gain 0
  {679, 1607}, // Gain 1
  {510, 1208}, // Gain 2
  {411, 973},  // Gain 3
  {274, 648},  // Gain 4
  {243, 575},  // Gain 5
  {206, 487},  // Gain 6
  {143, 339}   // Gain 7
};

// Presently unused.
float GAIN_SCALES[8] = {
  0.73,
  0.92,
  1.22,
  1.52,
  2.27,
  2.56,
  3.03,
  4.35
};

// Current gain configuration for the sensor.
int gain = 7;

float adjust_x(float x, float y, float z) {
  return x;
}

float adjust_z(float x, float y, float z) {
//  return z - 0.0025 * x;
  return z;
}

/* 
 *  Function: _compute_heading
 *  ---------------------------------------------------
 *  This method takes x and z dimensions and computes
 *  the directional heading using trig.
 */
float _compute_heading(float x, float y, float z) {
  // Apply the offsets to normalize the values based
  // on the plane it is mounted on and then compute the heading
  x = adjust_x(x + COMPASS_X_OFFSET, y + COMPASS_Y_OFFSET, z + COMPASS_Z_OFFSET);
  z = adjust_z(x, y + COMPASS_Y_OFFSET, z + COMPASS_Z_OFFSET);
  
  // Compute the angle in degrees
  float heading = atan2(z, x) * (180/M_PI);

  // Constrain the value
  if (heading < 0) {
    heading = 360 - abs(heading);
  }

  return (float)((int)heading % 360);
}

/* 
 *  Function: _assert_gain_value
 *  ---------------------------------------------------
 *  This method will take a character representing a
 *  dimension, and a value. And then check whether the
 *  value is within the min/max threshold of currently
 *  configured gain. If not, some debug info is output
 *  and this method returns false.
 *  
 *  Returns true if value is within threshold.
 */
bool _assert_gain_value(char dimension, int val) {
  int min_val = GAIN_LIMITS[gain][0];
  int max_val = GAIN_LIMITS[gain][1];
  if (val < min_val || val > max_val) {
    Serial.print("Gain ");
    Serial.print(gain);
    Serial.print(" failed axis ");
    Serial.print(dimension);
    Serial.print(" with value ");
    Serial.print(val);
    Serial.print("  - threshold <");
    Serial.print(min_val);
    Serial.print(", ");
    Serial.print(max_val);
    Serial.println(">");
    return false;
  } else {
    return true;
  }
}

/* 
 *  Function: init_compass
 *  ---------------------------------------------------
 *  This method will initialize the compass according
 *  to the datasheet by requesting the device enter
 *  single-value mode.
 */
void compass_init(void) {
  // Initialize continuous collection mode
  i2c_write_byte(COMPASS_DEVICE_ID, 0x00, 0x70);
  i2c_write_byte(COMPASS_DEVICE_ID, 0x01, COMPASS_GAINS[gain]);
  i2c_write_byte(COMPASS_DEVICE_ID, 0x02, 0x00);
  
  // Then delay 6ms according to datasheet
  delay(6);
}

/* 
 *  Function: self_test
 *  ---------------------------------------------------
 *  This method will enter the compass module into
 *  self-test mode which uses magic to create magnetic
 *  fields, allowing the sensor to derive
 */
void compass_self_test(void) {
  VectorData heading;

  // Enter self-test mode
  i2c_write_byte((uint8_t)COMPASS_DEVICE_ID, (uint8_t)0x00, (uint8_t)0x71);
  delay(6);

  // Test each gain
  for (gain = 7; gain >= 0; gain--) {
    bool exit_loop = false;
    
    for (int samples = 0; samples < 3; samples++) {
      // Take some readings, save the last one
      heading = compass_read_raw();
      delay(67);
      heading = compass_read_raw();
      delay(67);
      heading = compass_read_raw();
      delay(67);
  
      // Check that values are within tolerances
      bool success = _assert_gain_value('X', heading.x)
        && _assert_gain_value('Y', heading.y)
        && _assert_gain_value('Z', heading.z);
  
      if (success) {
        exit_loop = true;
        break;
      }
    }

    if (exit_loop) {
      break;
    }
  }

  if (gain < 0) {
    // ERROR
    Serial.println("ERROR: compass failed self-test");
    while(1) { }
  } else {
    Serial.print("Compass self-test succeeded! Gain configured at ");
    Serial.println(gain);
  }

  // Exit self-test mode
  i2c_write_byte(COMPASS_DEVICE_ID, 0x00, 0x70);
  delay(6);
  
  compass_init();
}

/* 
 *  Function: compass_read_raw
 *  ---------------------------------------------------
 *  This method will initiate an i2c_read_bytes
 *  operation against the compass peripheral and return
 *  the normalized X,Y,Z heading values.
 */
VectorData compass_read_raw(void) {
  // Read the bytes from device
  byte data[6];
  i2c_read_bytes(COMPASS_DEVICE_ID, 0x03, 6, data);  

  // Convert the values into their 16bit representations.
  // Apparently this'll just "work" for conversion into float datatype.
  int16_t x = ((int16_t)data[0] << 8) | (int16_t)data[1];
  int16_t z = ((int16_t)data[2] << 8) | (int16_t)data[3];
  int16_t y = ((int16_t)data[4] << 8) | (int16_t)data[5];

  // Finally, convert the integer into floats and return the resulting datastructure
  return VectorData {
    x: (float)x,
    y: (float)y, 
    z: (float)z,
    heading: _compute_heading(x, y, z),
  };
}

/* 
 *  Function: compass_read
 *  ---------------------------------------------------
 *  This method will initiate an i2c_read_bytes
 *  operation against the compass peripheral and then
 *  adjust the raw values based on calibration values
 *  to yield corrected readings.
 */
VectorData compass_read(void) {
  VectorData data = compass_read_raw();
  data.y += COMPASS_Y_OFFSET;
  data.x = adjust_x(data.x + COMPASS_X_OFFSET, data.y, data.z + COMPASS_Z_OFFSET);
  data.z = adjust_z(data.x, data.y, data.z + COMPASS_Z_OFFSET);

  float original_y = data.y;
  
  // Next, use the heading angle to discover the encoded 
  // calibration multiplier.
  float calibration_multiplier = COMPASS_MULTIPLIERS[(int)data.heading];

  // Now apply the multiplier
  data.y *= (calibration_multiplier / 10000);

  return data;    
}

HMC5883L.h

/*
 * compass.h
 * ------------------------------
 * This module defines all accelerometer commands
 */
#ifndef __ACCELEROMETER_H_
#define __ACCELEROMETER_H_

#include <Arduino.h>
#include "i2c.h"

#define COMPASS_DEVICE_ID (uint8_t)0x1E
#define COMPASS_X_OFFSET  50
#define COMPASS_Y_OFFSET  197
#define COMPASS_Z_OFFSET  21

struct VectorData {
  float x;
  float y;
  float z;
  float heading;
};

void compass_init(void);
void compass_self_test(void);
VectorData compass_read_raw(void);
VectorData compass_read(void);

#endif

i2c.cpp

#include "i2c.h"

/* 
 *  Function: i2c_write_command
 *  ---------------------------------------------------
 *  This method takes an i2c device_id and a single byte command
 *  and transmits it.
 *  
 *  This function returns an i2c_status_t object defining the success state of
 *  the transmission operation.
  */
i2c_status_t i2c_write_command(uint8_t device_id, uint8_t command) {
  Wire.beginTransmission(device_id);
  Wire.write(byte(command));
  return (i2c_status_t)Wire.endTransmission();
}

/* 
 *  Function: i2c_write_byte
 *  ---------------------------------------------------
 *  This method takes an i2c device_id, a destination register, and a data byte.
 *  It will then transmit both bytes - register and data. For certain peripherals
 *  you must specify the receiving register address and the value to write to it.
 *  
 *  This function returns an i2c_status_t object defining the success state of
 *  the transmission operation.
  */
i2c_status_t i2c_write_byte(uint8_t device_id, uint8_t reg_addr, uint8_t data) {
  Wire.beginTransmission(device_id);
  Wire.write(byte(reg_addr));
  Wire.write(byte(data));
  return (i2c_status_t)Wire.endTransmission();
}

/* 
 *  Function: i2c_read_byte
 *  ---------------------------------------------------
 *  This method takes a device_id and a register address and will
 *  update the i2c device so it points to the register, then
 *  request a single byte of data and return the value.
  */
byte i2c_read_byte(uint8_t device_id, uint8_t reg_addr) {
  Wire.beginTransmission(device_id);
  Wire.write(byte(reg_addr));
  Wire.endTransmission(false);
  Wire.requestFrom(device_id, 1);
  byte result = 0x00;
  while (Wire.available()) {
    result = Wire.read();
  }
  return result;
}

/* 
 *  Function: i2c_read_bytes
 *  ---------------------------------------------------
 *  This method takes a device_id, register address,
 *  quantity of bytes, and destination buffer. It
 *  will then point the i2c device to the register
 *  and request a number of bytes from the device equal
 *  to qty_of_bytes. The result is stored in the
 *  destination buffer.
 *  
 *  The return value of this function is the i2c_status_t
 *  object representing the status of the register 
 *  transmission operation.
  */
i2c_status_t i2c_read_bytes(uint8_t device_id, uint8_t reg_addr, uint8_t qty_of_bytes, uint8_t *destination) {
  Wire.beginTransmission(device_id);
  Wire.write(byte(reg_addr));
  i2c_status_t transmission_result = (i2c_status_t)Wire.endTransmission(false);
  Wire.requestFrom(device_id, qty_of_bytes);
  uint8_t index = 0;
  while (Wire.available()) {
    *(destination + index) = Wire.read();
    index += 1;
  }
  return transmission_result;
}

i2c.h

/*
 * i2c.h
 * ------------------------------
 * This module defines convenience methods surrounding
 * i2c communication.
 */
#ifndef __I2C_H_
#define __I2C_H_

#include <Arduino.h>
#include <Wire.h>

typedef enum i2c_status_t {
  SUCCESS = 0,
  ERROR_DATA_TOO_LONG = 1,
  ERROR_NACK_ON_ADDR_TX = 2,
  ERROR_NACK_ON_DATA_TX = 3,
  ERROR_OTHER = 4
} i2c_status_t;

i2c_status_t i2c_write_command(uint8_t device_id, uint8_t command);
i2c_status_t i2c_write_byte(uint8_t device_id, uint8_t reg_addr, uint8_t data);
byte i2c_read_byte(uint8_t device_id, uint8_t reg_addr);
i2c_status_t i2c_read_bytes(uint8_t device_id, uint8_t reg_addr, uint8_t qty_of_bytes, uint8_t *destination);

#endif