Tyler Weaver / AHRS

AHRS library, modified version of Peter Bartz work.

Dependencies:   MODSERIAL

Dependents:   AHRS_demo

Compass.cpp

Committer:
tylerjw
Date:
2012-11-08
Revision:
1:da3b20b5d38a
Parent:
0:014ee3239c80

File content as of revision 1:da3b20b5d38a:

/* This file is part of the Razor AHRS Firmware */
#include "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);
}