96 lines
3.2 KiB
C++
96 lines
3.2 KiB
C++
// Reference the I2C Library
|
||
#include <Wire.h>
|
||
// Reference the HMC5883L Compass Library
|
||
#include <HMC5883L.h>
|
||
|
||
// Store our compass as a variable.
|
||
HMC5883L compass;
|
||
// Record any errors that may occur in the compass.
|
||
int error = 0;
|
||
|
||
// Out setup routine, here we will configure the microcontroller and compass.
|
||
void setup()
|
||
{
|
||
// Initialize the serial port.
|
||
Serial.begin(38400);
|
||
|
||
Serial.println("Starting the I2C interface.");
|
||
Wire.begin(); // Start the I2C interface.
|
||
|
||
Serial.println("Constructing new HMC5883L");
|
||
compass = HMC5883L(); // Construct a new HMC5883 compass.
|
||
|
||
Serial.println("Setting scale to +/- 1.3 Ga");
|
||
error = compass.SetScale(1.3); // Set the scale of the compass.
|
||
if(error != 0) // If there is an error, print it out.
|
||
Serial.println(compass.GetErrorText(error));
|
||
|
||
Serial.println("Setting measurement mode to continous.");
|
||
error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
|
||
if(error != 0) // If there is an error, print it out.
|
||
Serial.println(compass.GetErrorText(error));
|
||
}
|
||
|
||
// Our main program loop.
|
||
void loop()
|
||
{
|
||
// Retrive the raw values from the compass (not scaled).
|
||
MagnetometerRaw raw = compass.ReadRawAxis();
|
||
// Retrived the scaled values from the compass (scaled to the configured scale).
|
||
MagnetometerScaled scaled = compass.ReadScaledAxis();
|
||
|
||
// Values are accessed like so:
|
||
int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)
|
||
|
||
// Calculate heading when the magnetometer is level, then correct for signs of axis.
|
||
float heading = atan2(scaled.YAxis, scaled.XAxis);
|
||
|
||
// Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
|
||
// Find yours here: http://www.magnetic-declination.com/
|
||
// Mine is: 2<> 37' W, which is 2.617 Degrees, or (which we need) 0.0456752665 radians, I will use 0.0457
|
||
// If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
|
||
//float declinationAngle = 0.0457;
|
||
//heading += declinationAngle;
|
||
|
||
// Correct for when signs are reversed.
|
||
if(heading < 0)
|
||
heading += 2*PI;
|
||
|
||
// Check for wrap due to addition of declination.
|
||
if(heading > 2*PI)
|
||
heading -= 2*PI;
|
||
|
||
// Convert radians to degrees for readability.
|
||
float headingDegrees = heading * 180/M_PI;
|
||
|
||
// Output the data via the serial port.
|
||
Output(raw, scaled, headingDegrees);
|
||
|
||
// Normally we would delay the application by 66ms to allow the loop
|
||
// to run at 15Hz (default bandwidth for the HMC5883L).
|
||
// However since we have a long serial out (104ms at 9600) we will let
|
||
// it run at its natural speed.
|
||
// delay(66);
|
||
}
|
||
|
||
// Output the data down the serial port.
|
||
void Output(MagnetometerRaw raw, MagnetometerScaled scaled, float headingDegrees)
|
||
{
|
||
Serial.print("Raw:\t");
|
||
Serial.print(raw.XAxis);
|
||
Serial.print(" ");
|
||
Serial.print(raw.YAxis);
|
||
Serial.print(" ");
|
||
Serial.print(raw.ZAxis);
|
||
Serial.print(" \tScaled:\t");
|
||
|
||
Serial.print(scaled.XAxis);
|
||
Serial.print(" ");
|
||
Serial.print(scaled.YAxis);
|
||
Serial.print(" ");
|
||
Serial.print(scaled.ZAxis);
|
||
|
||
Serial.print(headingDegrees);
|
||
Serial.println(" Degrees \t");
|
||
}
|