Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 07:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

needs some help

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1001 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-12-02 05:54:02 +00:00
parent 559229a667
commit 729cf47b3c
8 changed files with 87 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_Compass/AP_Compass.cpp
View File

@ -14,9 +14,9 @@
heading : Magnetic heading heading : Magnetic heading
heading_X : Magnetic heading X component heading_X : Magnetic heading X component
heading_Y : Magnetic heading Y component heading_Y : Magnetic heading Y component
mag_X : Raw X axis magnetometer data mag_x : Raw X axis magnetometer data
mag_Y : Raw Y axis magnetometer data mag_y : Raw Y axis magnetometer data
mag_Z : Raw Z axis magnetometer data mag_z : Raw Z axis magnetometer data
Methods: Methods:
init() : initialization of I2C and sensor init() : initialization of I2C and sensor
@ -78,13 +78,13 @@ AP_Compass::update()
// All bytes received? // All bytes received?
if (i == 6) { if (i == 6) {
// MSB byte first, then LSB, X,Y,Z // MSB byte first, then LSB, X,Y,Z
mag_X = -((((int)buff[0]) << 8) | buff[1]); // X axis mag_x = -((((int)buff[0]) << 8) | buff[1]); // X axis
mag_Y = ((((int)buff[2]) << 8) | buff[3]); // Y axis mag_y = ((((int)buff[2]) << 8) | buff[3]); // Y axis
mag_Z = -((((int)buff[4]) << 8) | buff[5]); // Z axis mag_z = -((((int)buff[4]) << 8) | buff[5]); // Z axis
} }
mag.x = mag_X; mag.x = mag_x;
mag.y = mag_Y; mag.y = mag_y;
mag.z = mag_Z mag.z = mag_z
} }
void void
@ -103,10 +103,10 @@ AP_Compass::calculate(float roll, float pitch)
sin_pitch = sin(pitch); sin_pitch = sin(pitch);
// Tilt compensated Magnetic field X component: // 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: // 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 // Magnetic heading
heading = atan2(-head_Y, head_X); heading = atan2(-head_Y, head_X);

20
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -20,11 +20,11 @@ AP_Compass_HIL::AP_Compass_HIL() : orientation(0), declination(0.0)
offset[2] = 0; offset[2] = 0;
// initialise orientation matrix // initialise orientation matrix
orientationMatrix = ROTATION_NONE; orientation_matrix = ROTATION_NONE;
} }
// Public Methods ////////////////////////////////////////////////////////////// // 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 unsigned long currentTime = millis(); // record current time
int numAttempts = 0; int numAttempts = 0;
@ -83,7 +83,7 @@ void AP_Compass_HIL::calculate(float roll, float pitch)
if( orientation == 0 ) if( orientation == 0 )
rotMagVec = Vector3f(magX+offset[0],magY+offset[1],magZ+offset[2]); rotMagVec = Vector3f(magX+offset[0],magY+offset[1],magZ+offset[2]);
else 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: // Tilt compensated Magnetic field X component:
headX = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch; 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 // Optimization for external DCM use. Calculate normalized components
headingX = cos(heading); heading_x = cos(heading);
headingY = sin(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; orientation_matrix = rotation_matrix;
if( orientationMatrix == ROTATION_NONE ) if( orientation_matrix == ROTATION_NONE )
orientation = 0; orientation = 0;
else else
orientation = 1; 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[0] = x;
offset[1] = y; offset[1] = y;
offset[2] = z; offset[2] = z;
} }
void AP_Compass_HIL::setDeclination(float radians) void AP_Compass_HIL::set_declination(float radians)
{ {
declination = radians; declination = radians;
} }

28
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -7,21 +7,23 @@
class AP_Compass_HIL : public Compass class AP_Compass_HIL : public Compass
{ {
private:
int orientation;
Matrix3f orientationMatrix;
float calibration[3];
int offset[3];
float declination;
public: public:
AP_Compass_HIL(); // Constructor AP_Compass_HIL(); // Constructor
bool init(int initialiseWireLib = 1);
void read(); bool init(int initialise_wire_lib = 1);
void calculate(float roll, float pitch); void read();
void setOrientation(const Matrix3f &rotationMatrix); void calculate(float roll, float pitch);
void setOffsets(int x, int y, int z); void set_orientation(const Matrix3f &rotation_matrix);
void setDeclination(float radians); void set_offsets(int x, int y, int z);
void setHIL(float Mag_X, float Mag_Y, float Mag_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 #endif

52
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -13,20 +13,20 @@
Variables: Variables:
heading : magnetic heading heading : magnetic heading
headingX : magnetic heading X component heading_x : magnetic heading X component
headingY : magnetic heading Y component heading_y : magnetic heading Y component
magX : Raw X axis magnetometer data magX : Raw X axis magnetometer data
magY : Raw Y axis magnetometer data magY : Raw Y axis magnetometer data
magZ : Raw Z 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: Methods:
init() : Initialization of I2C and sensor init() : Initialization of I2C and sensor
read() : Read Sensor data read() : Read Sensor data
calculate(float roll, float pitch) : Calculate tilt adjusted heading calculate(float roll, float pitch) : Calculate tilt adjusted heading
setOrientation(const Matrix3f &rotationMatrix) : Set orientation of compass set_orientation(const Matrix3f &rotation_matrix) : Set orientation of compass
setOffsets(int x, int y, int z) : Set adjustments for HardIron disturbances set_offsets(int x, int y, int z) : Set adjustments for HardIron disturbances
setDeclination(float radians) : Set heading adjustment between true north and magnetic north set_declination(float radians) : Set heading adjustment between true north and magnetic north
To do : code optimization To do : code optimization
Mount position : UPDATED Mount position : UPDATED
@ -41,7 +41,7 @@
#include <Wire.h> #include <Wire.h>
#include "AP_Compass_HMC5843.h" #include "AP_Compass_HMC5843.h"
#define CompassAddress 0x1E #define COMPASS_ADDRESS 0x1E
#define ConfigRegA 0x00 #define ConfigRegA 0x00
#define ConfigRegB 0x01 #define ConfigRegB 0x01
#define magGain 0x20 #define magGain 0x20
@ -61,11 +61,11 @@ AP_Compass_HMC5843::AP_Compass_HMC5843() : orientation(0), declination(0.0)
offset[2] = 0; offset[2] = 0;
// initialise orientation matrix // initialise orientation matrix
orientationMatrix = ROTATION_NONE; orientation_matrix = ROTATION_NONE;
} }
// Public Methods ////////////////////////////////////////////////////////////// // 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 unsigned long currentTime = millis(); // record current time
int numAttempts = 0; int numAttempts = 0;
@ -87,7 +87,7 @@ bool AP_Compass_HMC5843::init(int initialiseWireLib)
numAttempts++; numAttempts++;
// force positiveBias (compass should return 715 for all channels) // force positiveBias (compass should return 715 for all channels)
Wire.beginTransmission(CompassAddress); Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(ConfigRegA); Wire.send(ConfigRegA);
Wire.send(PositiveBiasConfig); Wire.send(PositiveBiasConfig);
if (0 != Wire.endTransmission()) if (0 != Wire.endTransmission())
@ -95,13 +95,13 @@ bool AP_Compass_HMC5843::init(int initialiseWireLib)
delay(50); delay(50);
// set gains // set gains
Wire.beginTransmission(CompassAddress); Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(ConfigRegB); Wire.send(ConfigRegB);
Wire.send(magGain); Wire.send(magGain);
Wire.endTransmission(); Wire.endTransmission();
delay(10); delay(10);
Wire.beginTransmission(CompassAddress); Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(ModeRegister); Wire.send(ModeRegister);
Wire.send(SingleConversion); Wire.send(SingleConversion);
Wire.endTransmission(); Wire.endTransmission();
@ -123,13 +123,13 @@ bool AP_Compass_HMC5843::init(int initialiseWireLib)
} }
// leave test mode // leave test mode
Wire.beginTransmission(CompassAddress); Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(ConfigRegA); Wire.send(ConfigRegA);
Wire.send(NormalOperation); Wire.send(NormalOperation);
Wire.endTransmission(); Wire.endTransmission();
delay(50); delay(50);
Wire.beginTransmission(CompassAddress); Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(ModeRegister); Wire.send(ModeRegister);
Wire.send(ContinuousConversion); // Set continuous mode (default to 10Hz) Wire.send(ContinuousConversion); // Set continuous mode (default to 10Hz)
Wire.endTransmission(); // End transmission Wire.endTransmission(); // End transmission
@ -144,12 +144,12 @@ void AP_Compass_HMC5843::read()
int i = 0; int i = 0;
byte buff[6]; byte buff[6];
Wire.beginTransmission(CompassAddress); Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(0x03); //sends address to read from Wire.send(0x03); //sends address to read from
Wire.endTransmission(); //end transmission Wire.endTransmission(); //end transmission
//Wire.beginTransmission(CompassAddress); //Wire.beginTransmission(COMPASS_ADDRESS);
Wire.requestFrom(CompassAddress, 6); // request 6 bytes from device Wire.requestFrom(COMPASS_ADDRESS, 6); // request 6 bytes from device
while(Wire.available()) while(Wire.available())
{ {
buff[i] = Wire.receive(); // receive one byte 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 magX = -((((int)buff[0]) << 8) | buff[1]) * calibration[0]; // X axis
magY = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y axis magY = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y axis
magZ = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z 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 ) if( orientation == 0 )
rotmagVec = Vector3f(magX+offset[0],magY+offset[1],magZ+offset[2]); rotmagVec = Vector3f(magX+offset[0],magY+offset[1],magZ+offset[2]);
else 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: // Tilt compensated magnetic field X component:
headX = rotmagVec.x*cos_pitch+rotmagVec.y*sin_roll*sin_pitch+rotmagVec.z*cos_roll*sin_pitch; 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 // Optimization for external DCM use. Calculate normalized components
headingX = cos(heading); heading_x = cos(heading);
headingY = sin(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; orientation_matrix = rotation_matrix;
if( orientationMatrix == ROTATION_NONE ) if( orientation_matrix == ROTATION_NONE )
orientation = 0; orientation = 0;
else else
orientation = 1; 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[0] = x;
offset[1] = y; offset[1] = y;
offset[2] = z; offset[2] = z;
} }
void AP_Compass_HMC5843::setDeclination(float radians) void AP_Compass_HMC5843::set_declination(float radians)
{ {
declination = radians; declination = radians;
} }

8
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -62,7 +62,7 @@ class AP_Compass_HMC5843 : public Compass
{ {
private: private:
int orientation; int orientation;
Matrix3f orientationMatrix; Matrix3f orientation_matrix;
float calibration[3]; float calibration[3];
int offset[3]; int offset[3];
float declination; float declination;
@ -71,8 +71,8 @@ class AP_Compass_HMC5843 : public Compass
bool init(int initialiseWireLib = 1); bool init(int initialiseWireLib = 1);
void read(); void read();
void calculate(float roll, float pitch); void calculate(float roll, float pitch);
void setOrientation(const Matrix3f &rotationMatrix); void set_orientation(const Matrix3f &rotation_matrix);
void setOffsets(int x, int y, int z); void set_offsets(int x, int y, int z);
void setDeclination(float radians); void set_declination(float radians);
}; };
#endif #endif

21
libraries/AP_Compass/Compass.h
View File

@ -7,20 +7,19 @@
class Compass class Compass
{ {
public: public:
int magX; int mag_x;
int magY; int mag_y;
int magZ; int mag_z;
float heading; float heading;
float headingX; float heading_x;
float headingY; float heading_y;
unsigned long lastUpdate; unsigned long last_update;
// virtual bool init(int initialise_wire_lib = 1);
virtual bool init(int initialiseWireLib = 1);
virtual void read(); virtual void read();
virtual void calculate(float roll, float pitch); virtual void calculate(float roll, float pitch);
virtual void setOrientation(const Matrix3f &rotationMatrix); virtual void set_orientation(const Matrix3f &rotation_matrix);
virtual void setOffsets(int x, int y, int z); virtual void set_offsets(int x, int y, int z);
virtual void setDeclination(float radians); virtual void set_declination(float radians);
}; };
#endif #endif

16
libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde
View File

@ -4,9 +4,9 @@
*/ */
#include <Wire.h> #include <Wire.h>
#include <AP_Compass.h> // Compass Library #include <AP_Compass_HMC5843.h> // Compass Library
AP_Compass compass; AP_Compass_HMC5843 compass;
unsigned long timer; unsigned long timer;
@ -14,7 +14,7 @@ void setup()
{ {
compass.init(); // Initialization compass.init(); // Initialization
Serial.begin(38400); Serial.begin(38400);
Serial.println("AP Compass library test (HMC5843)"); Serial.println("AP Compass library test (HMC5843)");
delay(1000); delay(1000);
timer = millis(); timer = millis();
} }
@ -25,16 +25,16 @@ void loop()
if((millis()- timer) > 100){ if((millis()- timer) > 100){
timer = millis(); timer = millis();
compass.update(); compass.read();
compass.calculate(0, 0); // roll = 0, pitch = 0 for this example compass.calculate(0, 0); // roll = 0, pitch = 0 for this example
Serial.print("Heading:"); Serial.print("Heading:");
Serial.print(compass.ground_course,DEC); Serial.print(compass.heading, DEC);
Serial.print(" ("); Serial.print(" (");
Serial.print(compass.mag_X); Serial.print(compass.mag_x);
Serial.print(","); Serial.print(",");
Serial.print(compass.mag_Y); Serial.print(compass.mag_y);
Serial.print(","); Serial.print(",");
Serial.print(compass.mag_Z); Serial.print(compass.mag_z);
Serial.println(" )"); Serial.println(" )");
} }
} }

6
libraries/AP_Compass/keywords.txt
View File

@ -7,9 +7,9 @@ calculate KEYWORD2
heading KEYWORD2 heading KEYWORD2
heading_X KEYWORD2 heading_X KEYWORD2
heading_Y KEYWORD2 heading_Y KEYWORD2
mag_X KEYWORD2 mag_x KEYWORD2
mag_Y KEYWORD2 mag_y KEYWORD2
mag_Z KEYWORD2 mag_z KEYWORD2