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

added magnetometer orientation functionality to APM_Compass 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@560 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com 2010-09-26 13:54:24 +00:00
parent 5ba0c7328f
commit 16df19a26d
4 changed files with 85 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
libraries/APM_Compass/APM_Compass.cpp
View File

@ -38,8 +38,20 @@ extern "C" {
#define CompassAddress 0x1E
// constant rotation matrices
const Matrix3f rotation[8] = {
Matrix3f( 1, 0, 0, 0, 1, 0, 0 ,0, 1 ), // COMPONENTS_UP_PINS_BACK = no rotation
Matrix3f( 0, 1, 0, -1, 0, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_LEFT = rotation_yaw_270
Matrix3f( -1, 0, 0, 0, -1, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_FORWARD = rotation_yaw_180
Matrix3f( 0, -1, 0, 1, 0, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_RIGHT = rotation_yaw_90
Matrix3f( 1, 0, 0, 0, -1, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_BACK = rotation_roll_180
Matrix3f( 0, -1, 0, -1, 0, 0, 0, 0, -1 ),// COMPONENTS_DOWN_PINS_LEFT = rotation_roll_180_yaw_270
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
};
// Constructors ////////////////////////////////////////////////////////////////
APM_Compass_Class::APM_Compass_Class()
APM_Compass_Class::APM_Compass_Class() : orientation(0)
{
}
@ -73,12 +85,12 @@ void APM_Compass_Class::Read()
Wire.endTransmission(); //end transmission
if (i==6) // All bytes received?
{
{
// 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
}
}
}
void APM_Compass_Class::Calculate(float roll, float pitch)
@ -89,15 +101,22 @@ void APM_Compass_Class::Calculate(float roll, float pitch)
float sin_roll;
float cos_pitch;
float sin_pitch;
Vector3f rotMagVec;
cos_roll = cos(roll); // Optimizacion, se puede sacar esto de la matriz DCM?
sin_roll = sin(roll);
cos_pitch = cos(pitch);
sin_pitch = sin(pitch);
// rotate the magnetometer values depending upon orientation
rotMagVec = rotation[orientation]*Vector3f(Mag_X,Mag_Y,Mag_Z);
// 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;
Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.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;
Head_Y = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll;
// Magnetic Heading
Heading = atan2(-Head_Y,Head_X);
// Optimization for external DCM use. Calculate normalized components
@ -106,5 +125,41 @@ void APM_Compass_Class::Calculate(float roll, float pitch)
}
void APM_Compass_Class::SetOrientation(int 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;
}*/
}
// make one instance for the user to use
APM_Compass_Class APM_Compass;

13
libraries/APM_Compass/APM_Compass.h
View File

@ -1,6 +1,17 @@
#ifndef APM_Compass_h
#define APM_Compass_h
#include <AP_Math.h>
#define APM_COMPASS_COMPONENTS_UP_PINS_BACK 0
#define APM_COMPASS_COMPONENTS_UP_PINS_LEFT 1
#define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD 2
#define APM_COMPASS_COMPONENTS_UP_PINS_RIGHT 3
#define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK 4
#define APM_COMPASS_COMPONENTS_DOWN_PINS_LEFT 5
#define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD 6
#define APM_COMPASS_COMPONENTS_DOWN_PINS_RIGHT 7
class APM_Compass_Class
{
private:
@ -11,11 +22,13 @@ 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);
};
extern APM_Compass_Class APM_Compass;

1
libraries/APM_Compass/examples/APM_Compass_test/APM_Compass_test.pde
View File

@ -5,6 +5,7 @@
#include <Wire.h>
#include <APM_Compass.h> // Compass Library
#include <AP_Math.h> // Math library
#define ToDeg(x) (x*57.2957795131) // *180/pi

19
libraries/APM_Compass/keywords.txt
View File

@ -1,15 +1,16 @@
Compass KEYWORD1
AP_Compass KEYWORD1
APM_Compass KEYWORD1
init KEYWORD2
update KEYWORD2
calculate KEYWORD2
heading KEYWORD2
heading_X KEYWORD2
heading_Y KEYWORD2
mag_X KEYWORD2
mag_Y KEYWORD2
mag_Z KEYWORD2
Init KEYWORD2
Read KEYWORD2
Calculate KEYWORD2
SetOrientation KEYWORD2
Heading KEYWORD2
Heading_X KEYWORD2
Heading_Y KEYWORD2
Mag_X KEYWORD2
Mag_Y KEYWORD2
Mag_Z KEYWORD2