File content as of revision 4:5ab2bb2f062b:

/***************************************************************************
** https://github.com/adafruit/Adafruit_9DOF/blob/master/Adafruit_9DOF.cpp
** Modifed Adafruit's orientation code to get only the tilt compensated heading
 ***************************************************************************/
/***************************************************************************
  This is a library for the Adafruit 9DOF Breakout
  Designed specifically to work with the Adafruit 9DOF Breakout:
  http://www.adafruit.com/products/1714
  These displays use I2C to communicate, 2 pins are required to interface.
  Adafruit invests time and resources providing this open source code,
  please support Adafruit andopen-source hardware by purchasing products
  from Adafruit!
  Written by Kevin Townsend for Adafruit Industries.  
  BSD license, all text above must be included in any redistribution
 ***************************************************************************/

#include <math.h>
#include "Adafruit_9DOF.h"

#define SENSORS_GRAVITY_EARTH             (9.80665F) 
#define MILLIG_TO_M_PER_S2                (SENSORS_GRAVITY_EARTH/1000.0F) 
#define MILLIGAUSS_TO_MICROTESLA       (10.0F) 
/**************************************************************************/
/*!
    @brief  Populates the .roll/.pitch/.heading fields in the sensors_vec_t
            struct with the right angular data (in degree).
            The starting position is set by placing the object flat and
            pointing northwards (Z-axis pointing upward and X-axis pointing
            northwards).
            The orientation of the object can be modeled as resulting from
            3 consecutive rotations in turn: heading (Z-axis), pitch (Y-axis),
            and roll (X-axis) applied to the starting position.
    @param  accel   The vector variable containing the
                          data from the accelerometer
    @param  mag     The vector variable containing the
                          data from the magnetometer
    @param  orientation   The sensors_vec_t object that will have it's
                          .roll, .pitch and .heading fields populated
*/
/**************************************************************************/
bool fusionGetOrientation(int16_t *accel, int16_t *mag, tOrientation *orientation)
{
  /* Make sure the input is valid, not null, etc. */
  if ( accel == NULL) return false;
  if ( mag   == NULL) return false;
  if ( orientation  == NULL) return false;

  float const PI_F = 3.14159265F;
  
  double aX = ((double) accel[X]) * MILLIG_TO_M_PER_S2;
  double aY = ((double) accel[Y]) * MILLIG_TO_M_PER_S2;
  double aZ = ((double) accel[Z]) * MILLIG_TO_M_PER_S2;
    
  double mX = ((double) mag[X]) * MILLIGAUSS_TO_MICROTESLA;
  double mY = ((double) mag[Y]) * MILLIGAUSS_TO_MICROTESLA;
  double mZ = ((double) mag[Z]) * MILLIGAUSS_TO_MICROTESLA;
    

  /* roll: Rotation around the X-axis. -180 <= roll <= 180                                          */
  /* a positive roll angle is defined to be a clockwise rotation about the positive X-axis          */
  /*                                                                                                */
  /*                    y                                                                           */
  /*      roll = atan2(---)                                                                         */
  /*                    z                                                                           */
  /*                                                                                                */
  /* where:  y, z are returned value from accelerometer sensor                                      */
  orientation->roll = (float)atan2(aY, aZ);

  /* pitch: Rotation around the Y-axis. -180 <= roll <= 180                                         */
  /* a positive pitch angle is defined to be a clockwise rotation about the positive Y-axis         */
  /*                                                                                                */
    /*                                 x                                                        */
  /*             roll = atan(-----------------)                                               */
  /*                          sqrt(y^2 + z^2)                                                 */
  /* where:  x, y, z are returned value from accelerometer sensor                             */

  double t_pitch = (aY*aY) + (aZ*aZ);
  float signOfZ = aZ >= 0 ? 1.0F : -1.0F;
  orientation->pitch = (float)atan2(aX, signOfZ * sqrt(t_pitch));


  /* heading: Rotation around the Z-axis. -180 <= roll <= 180                                       */
  /* a positive heading angle is defined to be a clockwise rotation about the positive Z-axis       */
  /*                                                                                                */
  /*                                       z * sin(roll) - y * cos(roll)                            */
  /*   heading = atan2(--------------------------------------------------------------------------)  */
  /*                    x * cos(pitch) + y * sin(pitch) * sin(roll) + z * sin(pitch) * cos(roll))   */
  /*                                                                                                */
  /* where:  x, y, z are returned value from magnetometer sensor                                    */
  orientation->heading = (float)atan2(mZ * sin(orientation->roll) - mY * cos(orientation->roll), \
                                      mX * cos(orientation->pitch) + \
                                      mY * sin(orientation->pitch) * sin(orientation->roll) + \
                                      mZ * sin(orientation->pitch) * cos(orientation->roll));


 orientation->heading = (float)atan2(mY, mX);

  /* Convert angular data to degree */
  orientation->roll = orientation->roll * 180 / PI_F;
  orientation->pitch = orientation->pitch * 180 / PI_F;
  orientation->heading = orientation->heading * 180 / PI_F;

  return true;
}