52 lines
1.2 KiB
C++
52 lines
1.2 KiB
C++
#include <SmingCore.h>
|
|
#include <Libraries/HMC5883L/HMC5883L.h>
|
|
|
|
namespace
|
|
{
|
|
// class default I2C address is 0x1E
|
|
// specific I2C addresses may be passed as a parameter here
|
|
// this device only supports one I2C address (0x1E)
|
|
HMC5883L mag;
|
|
int16_t mx, my, mz;
|
|
|
|
SimpleTimer procTimer;
|
|
|
|
void readCompass()
|
|
{
|
|
// read raw heading measurements from device
|
|
mag.getHeading(&mx, &my, &mz);
|
|
|
|
// To calculate heading in degrees. 0 degree indicates North
|
|
float heading = atan2(my, mx);
|
|
if(heading < 0) {
|
|
heading += 2 * PI;
|
|
} else if(heading > 2 * PI) {
|
|
heading -= 2 * PI;
|
|
}
|
|
|
|
// display tab-separated gyro x/y/z values and heading
|
|
Serial << "mag:\t" << mx << '\t' << my << '\t' << mz << "\theading:\t" << heading * RAD_TO_DEG << endl;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
void init()
|
|
{
|
|
Serial.begin(SERIAL_BAUD_RATE); // 115200 by default
|
|
Serial.systemDebugOutput(false); // Disable debug output to serial
|
|
|
|
// join I2C bus (I2Cdev library doesn't do this automatically)
|
|
Wire.begin();
|
|
|
|
mag.initialize();
|
|
|
|
if(mag.testConnection()) {
|
|
Serial.println(_F("[Compass] Magnetometer found"));
|
|
} else {
|
|
Serial.println(_F("Can't connect to Magnetometer"));
|
|
}
|
|
|
|
// Start reading loop
|
|
procTimer.initializeMs<100>(readCompass).start();
|
|
}
|