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

243 lines
8.1 KiB
C++
Raw Blame History

/*
APM_Compass.cpp - Arduino Library for HMC5843 I2C Magnetometer
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
Sensor is conected to I2C port
Sensor is initialized in Continuos mode (10Hz)
Variables:
Heading : Magnetic heading
Heading_X : Magnetic heading X component
Heading_Y : Magnetic heading Y component
Mag_X : Raw X axis magnetometer data
Mag_Y : Raw Y axis magnetometer data
Mag_Z : Raw Z axis magnetometer data
Methods:
Init() : Initialization of I2C and sensor
Read() : Read Sensor data
To do : Calibration of the sensor, code optimization
Mount position : UPDATED
Big capacitor pointing backward, connector forward
*/
extern "C" {
// AVR LibC Includes
#include <math.h>
#include "WConstants.h"
}
#include <Wire.h>
#include "APM_Compass.h"
#include "../AP_Math/AP_Math.h"
#define CompassAddress 0x1E
#define ConfigRegA 0x00
#define ConfigRegB 0x01
#define MagGain 0x20
#define PositiveBiasConfig 0x11
#define NegativeBiasConfig 0x12
#define NormalOperation 0x10
#define ModeRegister 0x02
#define ContinuousConversion 0x00
#define SingleConversion 0x01
// constant rotation matrices
const Matrix3f rotation[16] = {
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_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_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 ////////////////////////////////////////////////////////////////
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 //////////////////////////////////////////////////////////////
void APM_Compass_Class::Init(void)
{
unsigned long currentTime = millis(); // record current time
int numAttempts = 0;
int success = 0;
Wire.begin();
delay(10);
// calibration initialisation
calibration[0] = 1.0;
calibration[1] = 1.0;
calibration[2] = 1.0;
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
void APM_Compass_Class::Read()
{
int i = 0;
byte buff[6];
Wire.beginTransmission(CompassAddress);
Wire.send(0x03); //sends address to read from
Wire.endTransmission(); //end transmission
//Wire.beginTransmission(CompassAddress);
Wire.requestFrom(CompassAddress, 6); // request 6 bytes from device
while(Wire.available())
{
buff[i] = Wire.receive(); // receive one byte
i++;
}
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]) * 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
}
}
void APM_Compass_Class::Calculate(float roll, float pitch)
{
float Head_X;
float Head_Y;
float cos_roll;
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
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]);
// Tilt compensated Magnetic field X component:
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 = 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);
}
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;
Reference in New Issue View Git Blame Copy Permalink