Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -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_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);

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;
// 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;
}

28
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -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

52
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -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 <Wire.h>
#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;
}

8
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -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

21
libraries/AP_Compass/Compass.h
View File

@ -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

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

@ -4,9 +4,9 @@
*/
#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;
@ -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(" )");
}
}

6
libraries/AP_Compass/keywords.txt
View File

@ -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