Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

fixed bug in APM_compass Init method (MagGain should be sent to ConfigRegB). Also repeats calibration if invalid values received from compass.

Browse Source 
fixed bugs in rotation matrices.  default orientation is now "Components up pins forward".

Added simple (but not perfect) method of figuring out offsets to APM_Compass_test.pde

git-svn-id: https://arducopter.googlecode.com/svn/trunk@666 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
rmackay9@yahoo.com 2010-10-17 01:34:57 +00:00
parent 614d43c905
commit e29ea04fb1
4 changed files with 144 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
libraries/APM_Compass/APM_Compass.cpp
View File

@ -42,6 +42,7 @@ extern "C" {
#define ConfigRegB 0x01
#define MagGain 0x20
#define PositiveBiasConfig 0x11
#define NegativeBiasConfig 0x12
#define NormalOperation 0x10
#define ModeRegister 0x02
#define ContinuousConversion 0x00
@ -49,22 +50,22 @@ extern "C" {
// constant rotation matrices
const Matrix3f rotation[16] = {
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.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( -0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_FORWARD_RIGHT = rotation_yaw_135
Matrix3f( 1, 0, 0, 0, 1, 0, 0 ,0, 1 ), // COMPONENTS_UP_PINS_FORWARD = no rotation
Matrix3f( 0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_FORWARD_RIGHT = rotation_yaw_45
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( 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.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( -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.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_BACK_RIGHT = rotation_yaw_135
Matrix3f( -1, 0, 0, 0, -1, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_BACK = rotation_yaw_180
Matrix3f( -0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1 ), // COMPONENTS_UP_PINS_BACK_LEFT = rotation_yaw_225
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_315
Matrix3f( 1, 0, 0, 0, -1, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_FORWARD = rotation_roll_180
Matrix3f( 0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_FORWARD_RIGHT = rotation_roll_180_yaw_45
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
Matrix3f( -0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_BACK_RIGHT = rotation_roll_180_yaw_135
Matrix3f( -1, 0, 0, 0, 1, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_BACK = rotation_pitch_180
Matrix3f( -0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1 ), // COMPONENTS_DOWN_PINS_BACK_LEFT = rotation_roll_180_yaw_225
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_315
};
// Constructors ////////////////////////////////////////////////////////////////
@ -79,6 +80,10 @@ APM_Compass_Class::APM_Compass_Class() : orientation(0), declination(0.0)
// Public Methods //////////////////////////////////////////////////////////////
void APM_Compass_Class::Init(void)
{
unsigned long currentTime = millis(); // record current time
int numAttempts = 0;
int success = 0;
Wire.begin();
delay(10);
@ -87,43 +92,60 @@ void APM_Compass_Class::Init(void)
calibration[0] = 1.0;
calibration[1] = 1.0;
calibration[2] = 1.0;
Wire.beginTransmission(CompassAddress);
Wire.send(ConfigRegA);
Wire.send(PositiveBiasConfig);
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
while( success == 0 && numAttempts < 5 )
{
// record number of attempts at initialisation
numAttempts++;
// force positiveBias (compass should return 715 for all channels)
Wire.beginTransmission(CompassAddress);
Wire.send(ConfigRegA);
Wire.send(PositiveBiasConfig);
Wire.endTransmission();
delay(50);
// set gains
Wire.beginTransmission(CompassAddress);
Wire.send(ConfigRegB);
Wire.send(MagGain);
Wire.endTransmission();
delay(10);
Wire.beginTransmission(CompassAddress);
Wire.send(ModeRegister);
Wire.send(SingleConversion);
Wire.endTransmission();
delay(10);
// read values from the compass
Read();
delay(10);
// calibrate
if( abs(Mag_X) > 500 && abs(Mag_X) < 1000 && abs(Mag_Y) > 500 && abs(Mag_Y) < 1000 && abs(Mag_Z) > 500 && abs(Mag_Z) < 1000)
{
calibration[0] = abs(715.0 / Mag_X);
calibration[1] = abs(715.0 / Mag_Y);
calibration[2] = abs(715.0 / Mag_Z);
// mark success
success = 1;
}
// leave test mode
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
delay(50);
}
}
// Read Sensor data
@ -151,6 +173,7 @@ void APM_Compass_Class::Read()
Mag_X = -((((int)buff[0]) << 8) | buff[1]) * calibration[0]; // X axis
Mag_Y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y axis
Mag_Z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z axis
lastUpdate = millis(); // record time of update
}
}
@ -170,7 +193,7 @@ void APM_Compass_Class::Calculate(float roll, float pitch)
sin_pitch = sin(pitch);
// rotate the magnetometer values depending upon orientation
if( orientation == APM_COMPASS_COMPONENTS_UP_PINS_BACK)
if( orientation == APM_COMPASS_COMPONENTS_UP_PINS_FORWARD)
rotMagVec = Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]);
else
rotMagVec = rotation[orientation]*Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]);

35
libraries/APM_Compass/APM_Compass.h
View File

@ -1,26 +1,26 @@
#ifndef APM_Compass_h
#define APM_Compass_h
#define APM_COMPASS_COMPONENTS_UP_PINS_BACK 0
#define APM_COMPONENTS_UP_PINS_BACK_LEFT 1
#define APM_COMPASS_COMPONENTS_UP_PINS_LEFT 2
#define APM_COMPONENTS_UP_PINS_FORWARD_LEFT 3
#define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD 4
#define APM_COMPONENTS_UP_PINS_FORWARD_RIGHT 5
#define APM_COMPASS_COMPONENTS_UP_PINS_RIGHT 6
#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
#define APM_COMPASS_COMPONENTS_UP_PINS_FORWARD 0
#define APM_COMPONENTS_UP_PINS_FORWARD_RIGHT 1
#define APM_COMPASS_COMPONENTS_UP_PINS_RIGHT 2
#define APM_COMPONENTS_UP_PINS_BACK_RIGHT 3
#define APM_COMPASS_COMPONENTS_UP_PINS_BACK 4
#define APM_COMPONENTS_UP_PINS_BACK_LEFT 5
#define APM_COMPASS_COMPONENTS_UP_PINS_LEFT 6
#define APM_COMPONENTS_UP_PINS_FORWARD_LEFT 7
#define APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD 8
#define APM_COMPONENTS_DOWN_PINS_FORWARD_RIGHT 9
#define APM_COMPASS_COMPONENTS_DOWN_PINS_RIGHT 10
#define APM_COMPONENTS_DOWN_PINS_BACK_RIGHT 11
#define APM_COMPASS_COMPONENTS_DOWN_PINS_BACK 12
#define APM_COMPONENTS_DOWN_PINS_BACK_LEFT 13
#define APM_COMPASS_COMPONENTS_DOWN_PINS_LEFT 14
#define APM_COMPONENTS_DOWN_PINS_FORWARD_LEFT 15
class APM_Compass_Class
{
private:
private:
int orientation;
float calibration[3];
int offset[3];
@ -32,6 +32,7 @@ class APM_Compass_Class
float Heading;
float Heading_X;
float Heading_Y;
unsigned long lastUpdate;
APM_Compass_Class(); // Constructor
void Init();

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

@ -1,42 +1,87 @@
/*
Example of APM_Compass library (HMC5843 sensor).
Code by Jordi MuÒoz and Jose Julio. DIYDrones.com
offsets displayed are calculated from the min/max values from each axis.
rotate the compass so as to produce the maximum and minimum values
after the offsets stop changing, edit the code to pass these offsets into
APM_Compass.SetOffsets.
*/
#include <Wire.h>
#include <APM_Compass.h> // Compass Library
#include <AP_Math.h> // Math library
#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi
unsigned long timer;
void setup()
{
APM_Compass.Init(); // Initialization
Serial.begin(38400);
Serial.println("Compass library test (HMC5843)");
APM_Compass.Init(); // Initialization
APM_Compass.SetOrientation(APM_COMPASS_COMPONENTS_UP_PINS_FORWARD); // set compass's orientation on aircraft
APM_Compass.SetOffsets(0,0,0); // set offsets to account for surrounding interference
APM_Compass.SetDeclination(ToRad(0.0)); // set local difference between magnetic north and true north
delay(1000);
timer = millis();
}
void loop()
{
float tmp;
static float min[3], max[3], offset[3];
if((millis()- timer) > 100)
{
{
timer = millis();
APM_Compass.Read();
APM_Compass.Calculate(0,0); // roll = 0, pitch = 0 for this example
// capture min
if( APM_Compass.Mag_X < min[0] )
min[0] = APM_Compass.Mag_X;
if( APM_Compass.Mag_Y < min[1] )
min[1] = APM_Compass.Mag_Y;
if( APM_Compass.Mag_Z < min[2] )
min[2] = APM_Compass.Mag_Z;
// capture max
if( APM_Compass.Mag_X > max[0] )
max[0] = APM_Compass.Mag_X;
if( APM_Compass.Mag_Y > max[1] )
max[1] = APM_Compass.Mag_Y;
if( APM_Compass.Mag_Z > max[2] )
max[2] = APM_Compass.Mag_Z;
// calculate offsets
offset[0] = -(max[0]+min[0])/2;
offset[1] = -(max[1]+min[1])/2;
offset[2] = -(max[2]+min[2])/2;
// display all to user
Serial.print("Heading:");
Serial.print(ToDeg(APM_Compass.Heading));
Serial.print(" (");
Serial.print(APM_Compass.Mag_X);
Serial.print(",");
Serial.print(APM_Compass.Mag_Y);
Serial.print(",");
Serial.print(",");
Serial.print(APM_Compass.Mag_Z);
Serial.println(" )");
}
Serial.print(")");
// display offsets
Serial.print("\t offsets(");
Serial.print(offset[0]);
Serial.print(",");
Serial.print(offset[1]);
Serial.print(",");
Serial.print(offset[2]);
Serial.print(")");
Serial.println();
}
}

1
libraries/APM_Compass/keywords.txt
View File

@ -13,6 +13,7 @@ Heading_Y KEYWORD2
Mag_X KEYWORD2
Mag_Y KEYWORD2
Mag_Z KEYWORD2
lastUpdate KEYWORD2