Luke Petre / Mbed 2 deprecated RazorAHRS

Port of http://dev.qu.tu-berlin.de/projects/sf-razor-9dof-ahrs to an mbed, tested with a 9DOF Sensor Stick, SEN-10724

Dependencies:   mbed

Compass.cpp

Committer:
lpetre
Date:
2011-12-28
Revision:
2:5aa75c3d8cc3
Parent:
0:9a72d42c0da3

File content as of revision 2:5aa75c3d8cc3:

/* This file is part of the Razor AHRS Firmware */
#include "Razor_AHRS.h"
#include <math.h>

void IMU::Compass_Heading()
{
  float mag_x;
  float mag_y;
  float cos_roll;
  float sin_roll;
  float cos_pitch;
  float sin_pitch;
  
  cos_roll = cos(roll);
  sin_roll = sin(roll);
  cos_pitch = cos(pitch);
  sin_pitch = sin(pitch);
  
  // Tilt compensated magnetic field X
  mag_x = magnetom[0]*cos_pitch + magnetom[1]*sin_roll*sin_pitch + magnetom[2]*cos_roll*sin_pitch;
  // Tilt compensated magnetic field Y
  mag_y = magnetom[1]*cos_roll - magnetom[2]*sin_roll;
  // Magnetic Heading
  MAG_Heading = atan2(-mag_y, mag_x);
}