mirror of https://github.com/ArduPilot/ardupilot
360 lines
10 KiB
C++
360 lines
10 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
|
|
/*
|
|
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
|
|
lastUpdate : the time of the last successful reading
|
|
|
|
Methods:
|
|
Init() : Initialization of I2C and sensor
|
|
Read() : Read Sensor data
|
|
Calculate(float roll, float pitch) : Calculate tilt adjusted heading
|
|
SetOrientation(const Matrix3f &rotationMatrix) : Set orientation of compass
|
|
SetOffsets(int x, int y, int z) : Set adjustments for HardIron disturbances
|
|
SetDeclination(float radians) : Set heading adjustment between true north and magnetic north
|
|
|
|
To do : 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"
|
|
|
|
#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
|
|
|
|
// 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;
|
|
|
|
// initialise orientation matrix
|
|
orientationMatrix = ROTATION_NONE;
|
|
}
|
|
|
|
// Public Methods //////////////////////////////////////////////////////////////
|
|
bool APM_Compass_Class::Init(int initialiseWireLib)
|
|
{
|
|
unsigned long currentTime = millis(); // record current time
|
|
int numAttempts = 0;
|
|
int success = 0;
|
|
|
|
if( initialiseWireLib != 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);
|
|
if (0 != Wire.endTransmission())
|
|
continue; // compass not responding on the bus
|
|
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] = fabs(715.0 / Mag_X);
|
|
calibration[1] = fabs(715.0 / Mag_Y);
|
|
calibration[2] = fabs(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);
|
|
}
|
|
return(success);
|
|
}
|
|
|
|
// 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 == 0 )
|
|
rotMagVec = Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]);
|
|
else
|
|
rotMagVec = orientationMatrix*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(const Matrix3f &rotationMatrix)
|
|
{
|
|
orientationMatrix = rotationMatrix;
|
|
if( orientationMatrix == ROTATION_NONE )
|
|
orientation = 0;
|
|
else
|
|
orientation = 1;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
// Constructors ////////////////////////////////////////////////////////////////
|
|
APM_Compass_HIL_Class::APM_Compass_HIL_Class() : orientation(0), declination(0.0)
|
|
{
|
|
// mag x y z offset initialisation
|
|
offset[0] = 0;
|
|
offset[1] = 0;
|
|
offset[2] = 0;
|
|
|
|
// initialise orientation matrix
|
|
orientationMatrix = ROTATION_NONE;
|
|
}
|
|
|
|
// Public Methods //////////////////////////////////////////////////////////////
|
|
bool APM_Compass_HIL_Class::Init(int initialiseWireLib)
|
|
{
|
|
unsigned long currentTime = millis(); // record current time
|
|
int numAttempts = 0;
|
|
int success = 0;
|
|
|
|
// 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++;
|
|
|
|
// 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] = fabs(715.0 / Mag_X);
|
|
calibration[1] = fabs(715.0 / Mag_Y);
|
|
calibration[2] = fabs(715.0 / Mag_Z);
|
|
|
|
// mark success
|
|
success = 1;
|
|
}
|
|
}
|
|
return(success);
|
|
}
|
|
|
|
// Read Sensor data
|
|
void APM_Compass_HIL_Class::Read()
|
|
{
|
|
// values set by setHIL function
|
|
}
|
|
|
|
void APM_Compass_HIL_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 == 0 )
|
|
rotMagVec = Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]);
|
|
else
|
|
rotMagVec = orientationMatrix*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_HIL_Class::SetOrientation(const Matrix3f &rotationMatrix)
|
|
{
|
|
orientationMatrix = rotationMatrix;
|
|
if( orientationMatrix == ROTATION_NONE )
|
|
orientation = 0;
|
|
else
|
|
orientation = 1;
|
|
}
|
|
|
|
void APM_Compass_HIL_Class::SetOffsets(int x, int y, int z)
|
|
{
|
|
offset[0] = x;
|
|
offset[1] = y;
|
|
offset[2] = z;
|
|
}
|
|
|
|
void APM_Compass_HIL_Class::SetDeclination(float radians)
|
|
{
|
|
declination = radians;
|
|
}
|
|
|
|
void APM_Compass_HIL_Class::setHIL(float _Mag_X, float _Mag_Y, float _Mag_Z)
|
|
{
|
|
// TODO: map floats to raw
|
|
Mag_X = _Mag_X;
|
|
Mag_Y = _Mag_Y;
|
|
Mag_Z = _Mag_Z;
|
|
}
|