aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

incorporated Jose's offset code into main compass library.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@636 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com 2010-10-09 23:22:46 +00:00
parent c5155aa623
commit 3c00a92d14
3 changed files with 38 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/APM_Compass/APM_Compass.cpp
View File

@ -68,8 +68,12 @@ const Matrix3f rotation[16] = {
};
// Constructors ////////////////////////////////////////////////////////////////
APM_Compass_Class::APM_Compass_Class() : orientation(0)
APM_Compass_Class::APM_Compass_Class() : orientation(0), declination(0.0)
{
// mag x y z offset initialisation
offset[0] = 0;
offset[1] = 0;
offset[2] = 0;
}
// Public Methods //////////////////////////////////////////////////////////////
@ -79,6 +83,7 @@ void APM_Compass_Class::Init(void)
delay(10);
// calibration initialisation
calibration[0] = 1.0;
calibration[1] = 1.0;
calibration[2] = 1.0;
@ -166,9 +171,9 @@ void APM_Compass_Class::Calculate(float roll, float pitch)
// rotate the magnetometer values depending upon orientation
if( orientation == APM_COMPASS_COMPONENTS_UP_PINS_BACK)
rotMagVec = Vector3f(Mag_X,Mag_Y,Mag_Z);
rotMagVec = Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]);
else
rotMagVec = rotation[orientation]*Vector3f(Mag_X,Mag_Y,Mag_Z);
rotMagVec = rotation[orientation]*Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]);
// Tilt compensated Magnetic field X component:
Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch;
@ -176,6 +181,17 @@ void APM_Compass_Class::Calculate(float roll, float pitch)
Head_Y = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll;
// Magnetic Heading
Heading = atan2(-Head_Y,Head_X);
// Declination correction (if supplied)
if( declination != 0.0 )
{
Heading = Heading + declination;
if (Heading > M_PI) // Angle normalization (-180 deg, 180 deg)
Heading -= (2.0 * M_PI);
else if (Heading < -M_PI)
Heading += (2.0 * M_PI);
}
// Optimization for external DCM use. Calculate normalized components
Heading_X = cos(Heading);
Heading_Y = sin(Heading);
@ -187,6 +203,17 @@ void APM_Compass_Class::SetOrientation(int newOrientation)
orientation = newOrientation;
}
void APM_Compass_Class::SetOffsets(int x, int y, int z)
{
offset[0] = x;
offset[1] = y;
offset[2] = z;
}
void APM_Compass_Class::SetDeclination(float radians)
{
declination = radians;
}
// make one instance for the user to use
APM_Compass_Class APM_Compass;

8
libraries/APM_Compass/APM_Compass.h
View File

@ -21,7 +21,10 @@
class APM_Compass_Class
{
private:
float calibration[3];
int orientation;
float calibration[3];
int offset[3];
float declination;
public:
int Mag_X;
int Mag_Y;
@ -29,13 +32,14 @@ class APM_Compass_Class
float Heading;
float Heading_X;
float Heading_Y;
int orientation;
APM_Compass_Class(); // Constructor
void Init();
void Read();
void Calculate(float roll, float pitch);
void SetOrientation(int newOrientation);
void SetOffsets(int x, int y, int z);
void SetDeclination(float radians);
};
extern APM_Compass_Class APM_Compass;

2
libraries/APM_Compass/keywords.txt
View File

@ -5,6 +5,8 @@ Init KEYWORD2
Read KEYWORD2
Calculate KEYWORD2
SetOrientation KEYWORD2
SetOffsets KEYWORDS2
SetDeclination KEYWORDS2
Heading KEYWORD2
Heading_X KEYWORD2
Heading_Y KEYWORD2