diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index e2a0ed5acd..837ac69a41 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -14,9 +14,9 @@ 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 + 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 @@ -78,13 +78,13 @@ AP_Compass::update() // 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 + 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 } - mag.x = mag_X; - mag.y = mag_Y; - mag.z = mag_Z + mag.x = mag_x; + mag.y = mag_y; + mag.z = mag_z } void @@ -103,10 +103,10 @@ AP_Compass::calculate(float roll, float 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; + 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; + head_Y = mag_y * cos_roll - mag_z * sin_roll; // Magnetic heading heading = atan2(-head_Y, head_X); diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp index 46f8a71605..8b5fc13e68 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.cpp +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -20,11 +20,11 @@ AP_Compass_HIL::AP_Compass_HIL() : orientation(0), declination(0.0) offset[2] = 0; // initialise orientation matrix - orientationMatrix = ROTATION_NONE; + orientation_matrix = ROTATION_NONE; } // Public Methods ////////////////////////////////////////////////////////////// -bool AP_Compass_HIL::init(int initialiseWireLib) +bool AP_Compass_HIL::init(int initialise_wire_lib) { unsigned long currentTime = millis(); // record current time int numAttempts = 0; @@ -83,7 +83,7 @@ void AP_Compass_HIL::calculate(float roll, float pitch) if( orientation == 0 ) rotMagVec = Vector3f(magX+offset[0],magY+offset[1],magZ+offset[2]); else - rotMagVec = orientationMatrix*Vector3f(magX+offset[0],magY+offset[1],magZ+offset[2]); + rotMagVec = orientation_matrix*Vector3f(magX+offset[0],magY+offset[1],magZ+offset[2]); // Tilt compensated Magnetic field X component: headX = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch; @@ -103,27 +103,27 @@ void AP_Compass_HIL::calculate(float roll, float pitch) } // Optimization for external DCM use. Calculate normalized components - headingX = cos(heading); - headingY = sin(heading); + heading_x = cos(heading); + heading_y = sin(heading); } -void AP_Compass_HIL::setOrientation(const Matrix3f &rotationMatrix) +void AP_Compass_HIL::set_orientation(const Matrix3f &rotation_matrix) { - orientationMatrix = rotationMatrix; - if( orientationMatrix == ROTATION_NONE ) + orientation_matrix = rotation_matrix; + if( orientation_matrix == ROTATION_NONE ) orientation = 0; else orientation = 1; } -void AP_Compass_HIL::setOffsets(int x, int y, int z) +void AP_Compass_HIL::set_offsets(int x, int y, int z) { offset[0] = x; offset[1] = y; offset[2] = z; } -void AP_Compass_HIL::setDeclination(float radians) +void AP_Compass_HIL::set_declination(float radians) { declination = radians; } diff --git a/libraries/AP_Compass/AP_Compass_HIL.h b/libraries/AP_Compass/AP_Compass_HIL.h index b22e6dbfc2..67a13c0545 100644 --- a/libraries/AP_Compass/AP_Compass_HIL.h +++ b/libraries/AP_Compass/AP_Compass_HIL.h @@ -7,21 +7,23 @@ class AP_Compass_HIL : public Compass { - private: - int orientation; - Matrix3f orientationMatrix; - float calibration[3]; - int offset[3]; - float declination; public: AP_Compass_HIL(); // Constructor - bool init(int initialiseWireLib = 1); - void read(); - void calculate(float roll, float pitch); - void setOrientation(const Matrix3f &rotationMatrix); - void setOffsets(int x, int y, int z); - void setDeclination(float radians); - void setHIL(float Mag_X, float Mag_Y, float Mag_Z); + + bool init(int initialise_wire_lib = 1); + void read(); + void calculate(float roll, float pitch); + void set_orientation(const Matrix3f &rotation_matrix); + void set_offsets(int x, int y, int z); + void set_declination(float radians); + void setHIL(float Mag_X, float Mag_Y, float Mag_Z); + + private: + int orientation; + Matrix3f orientation_matrix; + float calibration[3]; + int offset[3]; + float declination; }; #endif diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp index f901222628..24220acc51 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.cpp +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -13,20 +13,20 @@ Variables: heading : magnetic heading - headingX : magnetic heading X component - headingY : magnetic heading Y component + heading_x : magnetic heading X component + heading_y : magnetic heading Y component magX : Raw X axis magnetometer data magY : Raw Y axis magnetometer data magZ : Raw Z axis magnetometer data - lastUpdate : the time of the last successful reading + last_update : the time of the last successful reading Methods: init() : Initialization of I2C and sensor read() : Read Sensor data calculate(float roll, float pitch) : Calculate tilt adjusted heading - setOrientation(const Matrix3f &rotationMatrix) : Set orientation of compass - setOffsets(int x, int y, int z) : Set adjustments for HardIron disturbances - setDeclination(float radians) : Set heading adjustment between true north and magnetic north + set_orientation(const Matrix3f &rotation_matrix) : Set orientation of compass + set_offsets(int x, int y, int z) : Set adjustments for HardIron disturbances + set_declination(float radians) : Set heading adjustment between true north and magnetic north To do : code optimization Mount position : UPDATED @@ -41,7 +41,7 @@ #include #include "AP_Compass_HMC5843.h" -#define CompassAddress 0x1E +#define COMPASS_ADDRESS 0x1E #define ConfigRegA 0x00 #define ConfigRegB 0x01 #define magGain 0x20 @@ -61,11 +61,11 @@ AP_Compass_HMC5843::AP_Compass_HMC5843() : orientation(0), declination(0.0) offset[2] = 0; // initialise orientation matrix - orientationMatrix = ROTATION_NONE; + orientation_matrix = ROTATION_NONE; } // Public Methods ////////////////////////////////////////////////////////////// -bool AP_Compass_HMC5843::init(int initialiseWireLib) +bool AP_Compass_HMC5843::init(int initialise_wire_lib) { unsigned long currentTime = millis(); // record current time int numAttempts = 0; @@ -87,7 +87,7 @@ bool AP_Compass_HMC5843::init(int initialiseWireLib) numAttempts++; // force positiveBias (compass should return 715 for all channels) - Wire.beginTransmission(CompassAddress); + Wire.beginTransmission(COMPASS_ADDRESS); Wire.send(ConfigRegA); Wire.send(PositiveBiasConfig); if (0 != Wire.endTransmission()) @@ -95,13 +95,13 @@ bool AP_Compass_HMC5843::init(int initialiseWireLib) delay(50); // set gains - Wire.beginTransmission(CompassAddress); + Wire.beginTransmission(COMPASS_ADDRESS); Wire.send(ConfigRegB); Wire.send(magGain); Wire.endTransmission(); delay(10); - Wire.beginTransmission(CompassAddress); + Wire.beginTransmission(COMPASS_ADDRESS); Wire.send(ModeRegister); Wire.send(SingleConversion); Wire.endTransmission(); @@ -123,13 +123,13 @@ bool AP_Compass_HMC5843::init(int initialiseWireLib) } // leave test mode - Wire.beginTransmission(CompassAddress); + Wire.beginTransmission(COMPASS_ADDRESS); Wire.send(ConfigRegA); Wire.send(NormalOperation); Wire.endTransmission(); delay(50); - Wire.beginTransmission(CompassAddress); + Wire.beginTransmission(COMPASS_ADDRESS); Wire.send(ModeRegister); Wire.send(ContinuousConversion); // Set continuous mode (default to 10Hz) Wire.endTransmission(); // End transmission @@ -144,12 +144,12 @@ void AP_Compass_HMC5843::read() int i = 0; byte buff[6]; - Wire.beginTransmission(CompassAddress); + Wire.beginTransmission(COMPASS_ADDRESS); Wire.send(0x03); //sends address to read from Wire.endTransmission(); //end transmission - //Wire.beginTransmission(CompassAddress); - Wire.requestFrom(CompassAddress, 6); // request 6 bytes from device + //Wire.beginTransmission(COMPASS_ADDRESS); + Wire.requestFrom(COMPASS_ADDRESS, 6); // request 6 bytes from device while(Wire.available()) { buff[i] = Wire.receive(); // receive one byte @@ -163,7 +163,7 @@ void AP_Compass_HMC5843::read() magX = -((((int)buff[0]) << 8) | buff[1]) * calibration[0]; // X axis magY = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y axis magZ = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z axis - lastUpdate = millis(); // record time of update + last_update = millis(); // record time of update } } @@ -186,7 +186,7 @@ void AP_Compass_HMC5843::calculate(float roll, float pitch) if( orientation == 0 ) rotmagVec = Vector3f(magX+offset[0],magY+offset[1],magZ+offset[2]); else - rotmagVec = orientationMatrix*Vector3f(magX+offset[0],magY+offset[1],magZ+offset[2]); + rotmagVec = orientation_matrix*Vector3f(magX+offset[0],magY+offset[1],magZ+offset[2]); // Tilt compensated magnetic field X component: headX = rotmagVec.x*cos_pitch+rotmagVec.y*sin_roll*sin_pitch+rotmagVec.z*cos_roll*sin_pitch; @@ -206,27 +206,27 @@ void AP_Compass_HMC5843::calculate(float roll, float pitch) } // Optimization for external DCM use. Calculate normalized components - headingX = cos(heading); - headingY = sin(heading); + heading_x = cos(heading); + heading_y = sin(heading); } -void AP_Compass_HMC5843::setOrientation(const Matrix3f &rotationMatrix) +void AP_Compass_HMC5843::set_orientation(const Matrix3f &rotation_matrix) { - orientationMatrix = rotationMatrix; - if( orientationMatrix == ROTATION_NONE ) + orientation_matrix = rotation_matrix; + if( orientation_matrix == ROTATION_NONE ) orientation = 0; else orientation = 1; } -void AP_Compass_HMC5843::setOffsets(int x, int y, int z) +void AP_Compass_HMC5843::set_offsets(int x, int y, int z) { offset[0] = x; offset[1] = y; offset[2] = z; } -void AP_Compass_HMC5843::setDeclination(float radians) +void AP_Compass_HMC5843::set_declination(float radians) { declination = radians; } diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h index 346fac394d..0364b5c5a4 100644 --- a/libraries/AP_Compass/AP_Compass_HMC5843.h +++ b/libraries/AP_Compass/AP_Compass_HMC5843.h @@ -62,7 +62,7 @@ class AP_Compass_HMC5843 : public Compass { private: int orientation; - Matrix3f orientationMatrix; + Matrix3f orientation_matrix; float calibration[3]; int offset[3]; float declination; @@ -71,8 +71,8 @@ class AP_Compass_HMC5843 : public Compass bool init(int initialiseWireLib = 1); void read(); void calculate(float roll, float pitch); - void setOrientation(const Matrix3f &rotationMatrix); - void setOffsets(int x, int y, int z); - void setDeclination(float radians); + void set_orientation(const Matrix3f &rotation_matrix); + void set_offsets(int x, int y, int z); + void set_declination(float radians); }; #endif diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 5098a7088c..a09354638a 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -7,20 +7,19 @@ class Compass { public: - int magX; - int magY; - int magZ; + int mag_x; + int mag_y; + int mag_z; float heading; - float headingX; - float headingY; - unsigned long lastUpdate; + float heading_x; + float heading_y; + unsigned long last_update; - // - virtual bool init(int initialiseWireLib = 1); + virtual bool init(int initialise_wire_lib = 1); virtual void read(); virtual void calculate(float roll, float pitch); - virtual void setOrientation(const Matrix3f &rotationMatrix); - virtual void setOffsets(int x, int y, int z); - virtual void setDeclination(float radians); + virtual void set_orientation(const Matrix3f &rotation_matrix); + virtual void set_offsets(int x, int y, int z); + virtual void set_declination(float radians); }; #endif 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 index 1a69883789..cd1bd031e7 100644 --- a/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde +++ b/libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde @@ -4,9 +4,9 @@ */ #include -#include // Compass Library +#include // Compass Library -AP_Compass compass; +AP_Compass_HMC5843 compass; unsigned long timer; @@ -14,7 +14,7 @@ void setup() { compass.init(); // Initialization Serial.begin(38400); - Serial.println("AP Compass library test (HMC5843)"); + Serial.println("AP Compass library test (HMC5843)"); delay(1000); timer = millis(); } @@ -25,16 +25,16 @@ void loop() if((millis()- timer) > 100){ timer = millis(); - compass.update(); + compass.read(); compass.calculate(0, 0); // roll = 0, pitch = 0 for this example Serial.print("Heading:"); - Serial.print(compass.ground_course,DEC); + Serial.print(compass.heading, DEC); Serial.print(" ("); - Serial.print(compass.mag_X); + Serial.print(compass.mag_x); Serial.print(","); - Serial.print(compass.mag_Y); + Serial.print(compass.mag_y); Serial.print(","); - Serial.print(compass.mag_Z); + 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 index c53c5c878c..5741e14ca2 100644 --- a/libraries/AP_Compass/keywords.txt +++ b/libraries/AP_Compass/keywords.txt @@ -7,9 +7,9 @@ calculate KEYWORD2 heading KEYWORD2 heading_X KEYWORD2 heading_Y KEYWORD2 -mag_X KEYWORD2 -mag_Y KEYWORD2 -mag_Z KEYWORD2 +mag_x KEYWORD2 +mag_y KEYWORD2 +mag_z KEYWORD2