Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ported John's compass calibration code to trunk. 

also added 45 deg angles to orientation matrices to allow more flexibility in mounting the compass to the APM frame.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@565 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com 2010-09-28 13:21:38 +00:00
parent 4e91c0c7d6
commit e7d4273d52
2 changed files with 85 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
libraries/APM_Compass/APM_Compass.cpp
View File

@ -37,18 +37,34 @@ extern "C" {
#include "APM_Compass.h" #include "APM_Compass.h"
#include "../AP_Math/AP_Math.h" #include "../AP_Math/AP_Math.h"
#define CompassAddress 0x1E #define CompassAddress 0x1E
#define ConfigRegA 0x00
#define ConfigRegB 0x01
#define MagGain 0x20
#define PositiveBiasConfig 0x11
#define NormalOperation 0x10
#define ModeRegister 0x02
#define ContinuousConversion 0x00
#define SingleConversion 0x01
// constant rotation matrices // constant rotation matrices
const Matrix3f rotation[8] = { const Matrix3f rotation[16] = {
Matrix3f( 1, 0, 0, 0, 1, 0, 0 ,0, 1 ), // COMPONENTS_UP_PINS_BACK = no rotation Matrix3f( 1, 0, 0, 0, 1, 0, 0 ,0, 1 ), // COMPONENTS_UP_PINS_BACK = no rotation
Matrix3f( 0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1 ), //COMPONENTS_UP_PINS_BACK_LEFT = rotation_yaw_315
Matrix3f( 0, 1, 0, -1, 0, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_LEFT = rotation_yaw_270 Matrix3f( 0, 1, 0, -1, 0, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_LEFT = rotation_yaw_270
Matrix3f( -0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_FORWARD_LEFT = rotation_yaw_225
Matrix3f( -1, 0, 0, 0, -1, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_FORWARD = rotation_yaw_180 Matrix3f( -1, 0, 0, 0, -1, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_FORWARD = rotation_yaw_180
Matrix3f( -0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_FORWARD_RIGHT = rotation_yaw_135
Matrix3f( 0, -1, 0, 1, 0, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_RIGHT = rotation_yaw_90 Matrix3f( 0, -1, 0, 1, 0, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_RIGHT = rotation_yaw_90
Matrix3f( 0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_BACK_RIGHT = rotation_yaw_45
Matrix3f( 1, 0, 0, 0, -1, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_BACK = rotation_roll_180 Matrix3f( 1, 0, 0, 0, -1, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_BACK = rotation_roll_180
Matrix3f( 0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_BACK_LEFT = rotation_roll_180_yaw_315
Matrix3f( 0, -1, 0, -1, 0, 0, 0, 0, -1 ),// COMPONENTS_DOWN_PINS_LEFT = rotation_roll_180_yaw_270 Matrix3f( 0, -1, 0, -1, 0, 0, 0, 0, -1 ),// COMPONENTS_DOWN_PINS_LEFT = rotation_roll_180_yaw_270
Matrix3f( -0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_FORWARD_LEFT = rotation_roll_180_yaw_225
Matrix3f( -1, 0, 0, 0, 1, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_FORWARD = rotation_pitch_180 Matrix3f( -1, 0, 0, 0, 1, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_FORWARD = rotation_pitch_180
Matrix3f( 0, 1, 0, 1, 0, 0, 0, 0, -1 ) // COMPONENTS_DOWN_PINS_RIGHT = rotation_roll_180_yaw_90 Matrix3f( -0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_FORWARD_RIGHT = rotation_roll_180_yaw_135
Matrix3f( 0, 1, 0, 1, 0, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_RIGHT = rotation_roll_180_yaw_90
Matrix3f( 0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1 ) // COMPONENTS_DOWN_PINS_BACK_RIGHT = rotation_roll_180_yaw_45
}; };
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
@ -60,10 +76,49 @@ APM_Compass_Class::APM_Compass_Class() : orientation(0)
void APM_Compass_Class::Init(void) void APM_Compass_Class::Init(void)
{ {
Wire.begin(); Wire.begin();
delay(10);
calibration[0] = 1.0;
calibration[1] = 1.0;
calibration[2] = 1.0;
Wire.beginTransmission(CompassAddress); Wire.beginTransmission(CompassAddress);
Wire.send(0x02); Wire.send(ConfigRegA);
Wire.send(0x00); // Set continouos mode (default to 10Hz) Wire.send(PositiveBiasConfig);
Wire.endTransmission(); //end transmission Wire.endTransmission();
delay(50);
Wire.beginTransmission(CompassAddress);
Wire.send(ConfigRegA);
Wire.send(MagGain);
Wire.endTransmission();
delay(10);
Wire.beginTransmission(CompassAddress);
Wire.send(ModeRegister);
Wire.send(SingleConversion);
Wire.endTransmission();
delay(10);
Read();
delay(10);
calibration[0] = abs(715.0 / Mag_X);
calibration[1] = abs(715.0 / Mag_Y);
calibration[2] = abs(715.0 / Mag_Z);
Wire.beginTransmission(CompassAddress);
Wire.send(ConfigRegA);
Wire.send(NormalOperation);
Wire.endTransmission();
delay(50);
Wire.beginTransmission(CompassAddress);
Wire.send(ModeRegister);
Wire.send(ContinuousConversion); // Set continuous mode (default to 10Hz)
Wire.endTransmission(); // End transmission
} }
// Read Sensor data // Read Sensor data
@ -88,9 +143,9 @@ void APM_Compass_Class::Read()
if (i==6) // All bytes received? if (i==6) // All bytes received?
{ {
// 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]) * calibration[0]; // X axis
Mag_Y = ((((int)buff[2]) << 8) | buff[3]); // Y axis Mag_Y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y axis
Mag_Z = -((((int)buff[4]) << 8) | buff[5]); // Z axis Mag_Z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z axis
} }
} }
@ -110,13 +165,14 @@ void APM_Compass_Class::Calculate(float roll, float pitch)
sin_pitch = sin(pitch); sin_pitch = sin(pitch);
// rotate the magnetometer values depending upon orientation // rotate the magnetometer values depending upon orientation
rotMagVec = rotation[orientation]*Vector3f(Mag_X,Mag_Y,Mag_Z); if( orientation == APM_COMPASS_COMPONENTS_UP_PINS_BACK)
rotMagVec = Vector3f(Mag_X,Mag_Y,Mag_Z);
else
rotMagVec = rotation[orientation]*Vector3f(Mag_X,Mag_Y,Mag_Z);
// 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 = 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;
// Tilt compensated Magnetic field Y component: // Tilt compensated Magnetic field Y component:
//Head_Y = Mag_Y*cos_roll-Mag_Z*sin_roll;
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);
@ -129,36 +185,6 @@ void APM_Compass_Class::Calculate(float roll, float pitch)
void APM_Compass_Class::SetOrientation(int newOrientation) void APM_Compass_Class::SetOrientation(int newOrientation)
{ {
orientation = newOrientation; orientation = newOrientation;
/* select( orientation )
{
case APM_COMPASS_COMPONENTS_UP_PINS_BACK:
orientationMatrix = rotation_none;
break;
case APM_COMPASS_COMPONENTS_UP_PINS_LEFT:
orientationMatrix = rotation_yaw_270;
break;
case APM_COMPASS_COMPONENTS_UP_PINS_FORWARD:
orientationMatrix = rotation_yaw_180;
break;
case APM_COMPASS_COMPONENTS_UP_PINS_RIGHT:
orientationMatrix = rotation_yaw_90;
break;
case APM_COMPASS_COMPONENTS_DOWN_PINS_BACK:
orientationMatrix = rotation_roll_180;
break;
case APM_COMPASS_COMPONENTS_DOWN_PINS_LEFT:
orientationMatrix = rotation_roll_180_yaw_270;
break;
case APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD:
orientationMatrix = rotation_pitch_180;
break;
case APM_COMPASS_COMPONENTS_DOWN_PINS_RIGHT:
orientationMatrix = rotation_roll_180_yaw_90;
break;
default:
orientationMatrix = rotation_none;
break;
}*/
} }

23
libraries/APM_Compass/APM_Compass.h
View File

@ -2,17 +2,26 @@
#define APM_Compass_h #define APM_Compass_h
#define APM_COMPASS_COMPONENTS_UP_PINS_BACK 0 #define APM_COMPASS_COMPONENTS_UP_PINS_BACK 0
#define APM_COMPASS_COMPONENTS_UP_PINS_LEFT 1 #define APM_COMPONENTS_UP_PINS_BACK_LEFT 1
#define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD 2 #define APM_COMPASS_COMPONENTS_UP_PINS_LEFT 2
#define APM_COMPASS_COMPONENTS_UP_PINS_RIGHT 3 #define APM_COMPONENTS_UP_PINS_FORWARD_LEFT 3
#define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK 4 #define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD 4
#define APM_COMPASS_COMPONENTS_DOWN_PINS_LEFT 5 #define APM_COMPONENTS_UP_PINS_FORWARD_RIGHT 5
#define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD 6 #define APM_COMPASS_COMPONENTS_UP_PINS_RIGHT 6
#define APM_COMPASS_COMPONENTS_DOWN_PINS_RIGHT 7 #define APM_COMPONENTS_UP_PINS_BACK_RIGHT 7
#define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK 8
#define APM_COMPONENTS_DOWN_PINS_BACK_LEFT 9
#define APM_COMPASS_COMPONENTS_DOWN_PINS_LEFT 10
#define APM_COMPONENTS_DOWN_PINS_FORWARD_LEFT 11
#define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD 12
#define APM_COMPONENTS_DOWN_PINS_FORWARD_RIGHT 13
#define APM_COMPASS_COMPONENTS_DOWN_PINS_RIGHT 14
#define APM_COMPONENTS_DOWN_PINS_BACK_RIGHT 15
class APM_Compass_Class class APM_Compass_Class
{ {
private: private:
float calibration[3];
public: public:
int Mag_X; int Mag_X;
int Mag_Y; int Mag_Y;