diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp new file mode 100644 index 0000000000..27d41c13d6 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -0,0 +1,114 @@ +/* + AP_Compass.cpp - Arduino Library for HMC5843 I2C Magnetometer + Code by Jordi Muñoz and Jose Julio. DIYDrones.com + + This library is free software; you can redistribute it and / or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + Sensor is conected to I2C port + Sensor is initialized in Continuos mode (10Hz) + + Variables: + heading : Magnetic heading + heading_X : Magnetic heading X component + heading_Y : Magnetic heading Y component + mag_X : Raw X axis magnetometer data + mag_Y : Raw Y axis magnetometer data + mag_Z : Raw Z axis magnetometer data + + Methods: + init() : initialization of I2C and sensor + update() : update Sensor data + + To do : Calibration of the sensor, code optimization + Mount position : UPDATED + Big capacitor pointing backward, connector forward + +*/ + + +extern "C" { + // AVR LibC Includes + #include + #include "WConstants.h" +} + +#include +#include "AP_Compass.h" + +#define COMPASS_ADDRESS 0x1E + +// Constructors //////////////////////////////////////////////////////////////// +AP_Compass::AP_Compass() +{ +} + +// Public Methods ////////////////////////////////////////////////////////////// +void +AP_Compass::init(void) +{ + Wire.begin(); + Wire.beginTransmission(COMPASS_ADDRESS); + Wire.send(0x02); + Wire.send(0x00); // Set continouos mode (default to 10Hz) + Wire.endTransmission(); // end transmission +} + +// update Sensor data +void +AP_Compass::update() +{ + int i = 0; + byte buff[6]; + + Wire.beginTransmission(COMPASS_ADDRESS); + Wire.send(0x03); // sends address to read from + Wire.endTransmission(); // end transmission + + //Wire.beginTransmission(COMPASS_ADDRESS); + Wire.requestFrom(COMPASS_ADDRESS, 6); // request 6 bytes from device + while(Wire.available()){ + buff[i] = Wire.receive(); // receive one byte + i++; + } + Wire.endTransmission(); // end transmission + + // All bytes received? + if (i == 6) { + // MSB byte first, then LSB, X,Y,Z + mag_X = -((((int)buff[0]) << 8) | buff[1]); // X axis + mag_Y = ((((int)buff[2]) << 8) | buff[3]); // Y axis + mag_Z = -((((int)buff[4]) << 8) | buff[5]); // Z axis + } +} + +void +AP_Compass::calculate(float roll, float pitch) +{ + float head_X; + float head_Y; + float cos_roll; + float sin_roll; + float cos_pitch; + float sin_pitch; + + cos_roll = cos(roll); // Optimization, you can get this out of the matrix DCM? + sin_roll = sin(roll); + cos_pitch = cos(pitch); + sin_pitch = sin(pitch); + + // Tilt compensated Magnetic field X component: + head_X = mag_X * cos_pitch + mag_Y * sin_roll * sin_pitch + mag_Z * cos_roll * sin_pitch; + + // Tilt compensated Magnetic field Y component: + head_Y = mag_Y * cos_roll - mag_Z * sin_roll; + + // Magnetic heading + heading = atan2(-head_Y, head_X); + + // Optimization for external DCM use. calculate normalized components + heading_X = cos(heading); + heading_Y = sin(heading); +} diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h new file mode 100644 index 0000000000..c2130b44d7 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass.h @@ -0,0 +1,17 @@ +#ifndef AP_Compass_h +#define AP_Compass_h + +#include + +class AP_Compass : public Compass +{ + public: + AP_Compass(); // Constructor + void init(); + void update(); + void calculate(float roll, float pitch); + + private: +}; + +#endif \ No newline at end of file diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h new file mode 100644 index 0000000000..8ce8913b54 --- /dev/null +++ b/libraries/AP_Compass/Compass.h @@ -0,0 +1,22 @@ +#ifndef Compass_h +#define Compass_h + +#include + +class Compass +{ + public: + virtual void init(); + virtual void update(); + virtual void calculate(float roll, float pitch); + + int16_t mag_X; + int16_t mag_Y; + int16_t mag_Z; + float heading; + float heading_X; + float heading_Y; + +}; + +#endif \ No newline at end of file diff --git a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde new file mode 100644 index 0000000000..54776ebb4d --- /dev/null +++ b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde @@ -0,0 +1,40 @@ +/* + Example of APM_Compass library (HMC5843 sensor). + Code by Jordi MuÃ’oz and Jose Julio. DIYDrones.com +*/ + +#include +#include // Compass Library + +AP_Compass compass; + +unsigned long timer; + +void setup() +{ + compass.init(); // Initialization + Serial.begin(38400); + Serial.println("AP Compass library test (HMC5843)"); + delay(1000); + timer = millis(); +} + +void loop() +{ + float tmp; + + if((millis()- timer) > 100){ + timer = millis(); + compass.update(); + compass.calculate(0, 0); // roll = 0, pitch = 0 for this example + Serial.print("Heading:"); + Serial.print(degrees(compass.heading)); + Serial.print(" ("); + Serial.print(compass.mag_X); + Serial.print(","); + Serial.print(compass.mag_Y); + Serial.print(","); + Serial.print(compass.mag_Z); + Serial.println(); + } +} \ No newline at end of file diff --git a/libraries/AP_Compass/keywords.txt b/libraries/AP_Compass/keywords.txt new file mode 100644 index 0000000000..c53c5c878c --- /dev/null +++ b/libraries/AP_Compass/keywords.txt @@ -0,0 +1,15 @@ +Compass KEYWORD1 +AP_Compass KEYWORD1 +APM_Compass KEYWORD1 +init KEYWORD2 +update KEYWORD2 +calculate KEYWORD2 +heading KEYWORD2 +heading_X KEYWORD2 +heading_Y KEYWORD2 +mag_X KEYWORD2 +mag_Y KEYWORD2 +mag_Z KEYWORD2 + + +