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 //////////////////////////////////////////////////////////////// // 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 ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
@ -79,6 +83,7 @@ void APM_Compass_Class::Init(void)
delay(10); delay(10);
// calibration initialisation
calibration[0] = 1.0; calibration[0] = 1.0;
calibration[1] = 1.0; calibration[1] = 1.0;
calibration[2] = 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 // rotate the magnetometer values depending upon orientation
if( orientation == APM_COMPASS_COMPONENTS_UP_PINS_BACK) 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 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: // Tilt compensated Magnetic field X component:
Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch; 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; Head_Y = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll;
// Magnetic Heading // Magnetic Heading
Heading = atan2(-Head_Y,Head_X); 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 // Optimization for external DCM use. Calculate normalized components
Heading_X = cos(Heading); Heading_X = cos(Heading);
Heading_Y = sin(Heading); Heading_Y = sin(Heading);
@ -187,6 +203,17 @@ void APM_Compass_Class::SetOrientation(int newOrientation)
orientation = 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 // make one instance for the user to use
APM_Compass_Class APM_Compass; APM_Compass_Class APM_Compass;

8
libraries/APM_Compass/APM_Compass.h
View File

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

2
libraries/APM_Compass/keywords.txt
View File

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