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

Added HIL and HMC5843 compass libs.

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@998 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2010-12-02 05:19:38 +00:00
parent 110685f880
commit 559229a667
4 changed files with 474 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

137
libraries/AP_Compass/AP_Compass_HIL.cpp Normal file
View File

@ -0,0 +1,137 @@
/*
AP_Compass_HIL.cpp - Arduino Library for HIL model of HMC5843 I2C Magnetometer
Code by James Goppert. 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.
*/
#include "AP_Compass_HIL.h"
// Constructors ////////////////////////////////////////////////////////////////
AP_Compass_HIL::AP_Compass_HIL() : 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 AP_Compass_HIL::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(magX) > 500 && abs(magX) < 1000 && abs(magY) > 500 && abs(magY) < 1000 && abs(magZ) > 500 && abs(magZ) < 1000)
{
calibration[0] = fabs(715.0 / magX);
calibration[1] = fabs(715.0 / magY);
calibration[2] = fabs(715.0 / magZ);
// mark success
success = 1;
}
}
return(success);
}
// Read Sensor data
void AP_Compass_HIL::read()
{
// values set by setHIL function
}
void AP_Compass_HIL::calculate(float roll, float pitch)
{
float headX;
float headY;
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(magX+offset[0],magY+offset[1],magZ+offset[2]);
else
rotMagVec = orientationMatrix*Vector3f(magX+offset[0],magY+offset[1],magZ+offset[2]);
// Tilt compensated Magnetic field X component:
headX = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch;
// Tilt compensated Magnetic field Y component:
headY = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll;
// Magnetic heading
heading = atan2(-headY,headX);
// 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
headingX = cos(heading);
headingY = sin(heading);
}
void AP_Compass_HIL::setOrientation(const Matrix3f &rotationMatrix)
{
orientationMatrix = rotationMatrix;
if( orientationMatrix == ROTATION_NONE )
orientation = 0;
else
orientation = 1;
}
void AP_Compass_HIL::setOffsets(int x, int y, int z)
{
offset[0] = x;
offset[1] = y;
offset[2] = z;
}
void AP_Compass_HIL::setDeclination(float radians)
{
declination = radians;
}
void AP_Compass_HIL::setHIL(float _magX, float _magY, float _magZ)
{
// TODO: map floats to raw
magX = _magX;
magY = _magY;
magZ = _magZ;
}

27
libraries/AP_Compass/AP_Compass_HIL.h Normal file
View File

@ -0,0 +1,27 @@
#ifndef AP_Compass_HIL_H
#define AP_Compass_HIL_H
#include <Compass.h>
#include "AP_Compass_HMC5843.h" // to get #defines since we are modelling this
#include "WProgram.h"
class AP_Compass_HIL : public Compass
{
private:
int orientation;
Matrix3f orientationMatrix;
float calibration[3];
int offset[3];
float declination;
public:
AP_Compass_HIL(); // Constructor
bool init(int initialiseWireLib = 1);
void read();
void calculate(float roll, float pitch);
void setOrientation(const Matrix3f &rotationMatrix);
void setOffsets(int x, int y, int z);
void setDeclination(float radians);
void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
};
#endif

232
libraries/AP_Compass/AP_Compass_HMC5843.cpp Normal file
View File

@ -0,0 +1,232 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
/*
AP_Compass_HMC5843.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
headingX : magnetic heading X component
headingY : magnetic heading Y component
magX : Raw X axis magnetometer data
magY : Raw Y axis magnetometer data
magZ : 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
*/
// AVR LibC Includes
#include <math.h>
#include "WConstants.h"
#include <Wire.h>
#include "AP_Compass_HMC5843.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 ////////////////////////////////////////////////////////////////
AP_Compass_HMC5843::AP_Compass_HMC5843() : 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 AP_Compass_HMC5843::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(magX) > 500 && abs(magX) < 1000 && abs(magY) > 500 && abs(magY) < 1000 && abs(magZ) > 500 && abs(magZ) < 1000)
{
calibration[0] = fabs(715.0 / magX);
calibration[1] = fabs(715.0 / magY);
calibration[2] = fabs(715.0 / magZ);
// 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 AP_Compass_HMC5843::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
magX = -((((int)buff[0]) << 8) | buff[1]) * calibration[0]; // X axis
magY = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y axis
magZ = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z axis
lastUpdate = millis(); // record time of update
}
}
void AP_Compass_HMC5843::calculate(float roll, float pitch)
{
float headX;
float headY;
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(magX+offset[0],magY+offset[1],magZ+offset[2]);
else
rotmagVec = orientationMatrix*Vector3f(magX+offset[0],magY+offset[1],magZ+offset[2]);
// Tilt compensated magnetic field X component:
headX = rotmagVec.x*cos_pitch+rotmagVec.y*sin_roll*sin_pitch+rotmagVec.z*cos_roll*sin_pitch;
// Tilt compensated magnetic field Y component:
headY = rotmagVec.y*cos_roll-rotmagVec.z*sin_roll;
// magnetic heading
heading = atan2(-headY,headX);
// 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
headingX = cos(heading);
headingY = sin(heading);
}
void AP_Compass_HMC5843::setOrientation(const Matrix3f &rotationMatrix)
{
orientationMatrix = rotationMatrix;
if( orientationMatrix == ROTATION_NONE )
orientation = 0;
else
orientation = 1;
}
void AP_Compass_HMC5843::setOffsets(int x, int y, int z)
{
offset[0] = x;
offset[1] = y;
offset[2] = z;
}
void AP_Compass_HMC5843::setDeclination(float radians)
{
declination = radians;
}

78
libraries/AP_Compass/AP_Compass_HMC5843.h Normal file
View File

@ -0,0 +1,78 @@
#ifndef AP_Compass_HMC5843_H
#define AP_Compass_HMC5843_H
#include "Compass.h"
#include "../AP_Math/AP_Math.h"
// Rotation matrices
#define ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1)
#define ROTATION_YAW_45 Matrix3f(0.70710678, -0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, 1)
#define ROTATION_YAW_90 Matrix3f(0, -1, 0, 1, 0, 0, 0, 0, 1)
#define ROTATION_YAW_135 Matrix3f(-0.70710678, -0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, 1)
#define ROTATION_YAW_180 Matrix3f(-1, 0, 0, 0, -1, 0, 0, 0, 1)
#define ROTATION_YAW_225 Matrix3f(-0.70710678, 0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, 1)
#define ROTATION_YAW_270 Matrix3f(0, 1, 0, -1, 0, 0, 0, 0, 1)
#define ROTATION_YAW_315 Matrix3f(0.70710678, 0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, 1)
#define ROTATION_ROLL_180 Matrix3f(1, 0, 0, 0, -1, 0, 0, 0, -1)
#define ROTATION_ROLL_180_YAW_45 Matrix3f(0.70710678, 0.70710678, 0, 0.70710678, -0.70710678, 0, 0, 0, -1)
#define ROTATION_ROLL_180_YAW_90 Matrix3f(0, 1, 0, 1, 0, 0, 0, 0, -1)
#define ROTATION_ROLL_180_YAW_135 Matrix3f(-0.70710678, 0.70710678, 0, 0.70710678, 0.70710678, 0, 0, 0, -1)
#define ROTATION_PITCH_180 Matrix3f(-1, 0, 0, 0, 1, 0, 0, 0, -1)
#define ROTATION_ROLL_180_YAW_225 Matrix3f(-0.70710678, -0.70710678, 0, -0.70710678, 0.70710678, 0, 0, 0, -1)
#define ROTATION_ROLL_180_YAW_270 Matrix3f(0, -1, 0, -1, 0, 0, 0, 0, -1)
#define ROTATION_ROLL_180_YAW_315 Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1)
// orientations for DIYDrones magnetometer
#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE
#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_45
#define AP_COMPASS_COMPONENTS_UP_PINS_RIGHT ROTATION_YAW_90
#define AP_COMPASS_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_135
#define AP_COMPASS_COMPONENTS_UP_PINS_BACK ROTATION_YAW_180
#define AP_COMPASS_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_225
#define AP_COMPASS_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_270
#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_315
#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180
#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_45
#define AP_COMPASS_COMPONENTS_DOWN_PINS_RIGHT ROTATION_ROLL_180_YAW_90
#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_135
#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK ROTATION_PITCH_180
#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_225
#define AP_COMPASS_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180_YAW_270
#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_315
// orientations for Sparkfun magnetometer
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD ROTATION_YAW_270
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_RIGHT ROTATION_YAW_315
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_RIGHT ROTATION_NONE
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_RIGHT ROTATION_YAW_45
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK ROTATION_YAW_90
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK_LEFT ROTATION_YAW_135
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_180
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD_LEFT ROTATION_YAW_225
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180_YAW_90
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_RIGHT ROTATION_ROLL_180_YAW_135
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_RIGHT ROTATION_PITCH_180
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_RIGHT ROTATION_ROLL_180_YAW_225
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK ROTATION_ROLL_180_YAW_270
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_BACK_LEFT ROTATION_ROLL_180_YAW_315
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_LEFT ROTATION_ROLL_180
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_45
class AP_Compass_HMC5843 : public Compass
{
private:
int orientation;
Matrix3f orientationMatrix;
float calibration[3];
int offset[3];
float declination;
public:
AP_Compass_HMC5843(); // Constructor
bool init(int initialiseWireLib = 1);
void read();
void calculate(float roll, float pitch);
void setOrientation(const Matrix3f &rotationMatrix);
void setOffsets(int x, int y, int z);
void setDeclination(float radians);
};
#endif