From 559229a6676b6f5dcba12f6d348c379ce548f934 Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Thu, 2 Dec 2010 05:19:38 +0000 Subject: [PATCH] Added HIL and HMC5843 compass libs. git-svn-id: https://arducopter.googlecode.com/svn/trunk@998 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_Compass/AP_Compass_HIL.cpp | 137 ++++++++++++ libraries/AP_Compass/AP_Compass_HIL.h | 27 +++ libraries/AP_Compass/AP_Compass_HMC5843.cpp | 232 ++++++++++++++++++++ libraries/AP_Compass/AP_Compass_HMC5843.h | 78 +++++++ 4 files changed, 474 insertions(+) create mode 100644 libraries/AP_Compass/AP_Compass_HIL.cpp create mode 100644 libraries/AP_Compass/AP_Compass_HIL.h create mode 100644 libraries/AP_Compass/AP_Compass_HMC5843.cpp create mode 100644 libraries/AP_Compass/AP_Compass_HMC5843.h diff --git a/libraries/AP_Compass/AP_Compass_HIL.cpp b/libraries/AP_Compass/AP_Compass_HIL.cpp new file mode 100644 index 0000000000..46f8a71605 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_HIL.cpp @@ -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; +} diff --git a/libraries/AP_Compass/AP_Compass_HIL.h b/libraries/AP_Compass/AP_Compass_HIL.h new file mode 100644 index 0000000000..b22e6dbfc2 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_HIL.h @@ -0,0 +1,27 @@ +#ifndef AP_Compass_HIL_H +#define AP_Compass_HIL_H + +#include +#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 diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.cpp b/libraries/AP_Compass/AP_Compass_HMC5843.cpp new file mode 100644 index 0000000000..f901222628 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_HMC5843.cpp @@ -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 +#include "WConstants.h" + +#include +#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; +} diff --git a/libraries/AP_Compass/AP_Compass_HMC5843.h b/libraries/AP_Compass/AP_Compass_HMC5843.h new file mode 100644 index 0000000000..346fac394d --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_HMC5843.h @@ -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