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

Line ending property changes. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1649 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
DrZiplok@gmail.com 2011-02-14 04:27:07 +00:00
parent dee05d1838
commit b9152dc544
5 changed files with 361 additions and 361 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -1,29 +1,29 @@
/* /*
AP_Compass_HIL.cpp - Arduino Library for HIL model of HMC5843 I2C Magnetometer AP_Compass_HIL.cpp - Arduino Library for HIL model of HMC5843 I2C Magnetometer
Code by James Goppert. DIYDrones.com Code by James Goppert. DIYDrones.com
This library is free software; you can redistribute it and / or This library is free software; you can redistribute it and / or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. version 2.1 of the License, or (at your option) any later version.
*/ */
#include "AP_Compass_HIL.h" #include "AP_Compass_HIL.h"
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void AP_Compass_HIL::read() void AP_Compass_HIL::read()
{ {
// values set by setHIL function // values set by setHIL function
} }
// Update raw magnetometer values from HIL data // Update raw magnetometer values from HIL data
// //
void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z) void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z)
{ {
// TODO: map floats to raw // TODO: map floats to raw
mag_x = _mag_x; mag_x = _mag_x;
mag_y = _mag_y; mag_y = _mag_y;
mag_z = _mag_z; mag_z = _mag_z;
} }

32
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -1,16 +1,16 @@
#ifndef AP_Compass_HIL_H #ifndef AP_Compass_HIL_H
#define AP_Compass_HIL_H #define AP_Compass_HIL_H
#include "Compass.h" #include "Compass.h"
class AP_Compass_HIL : public Compass class AP_Compass_HIL : public Compass
{ {
public: public:
AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {} AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
virtual bool init(); virtual bool init();
virtual void read(); virtual void read();
void setHIL(float Mag_X, float Mag_Y, float Mag_Z); void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
}; };
#endif #endif

282
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -1,141 +1,141 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
/* /*
AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer
Code by Jordi Muñoz and Jose Julio. DIYDrones.com Code by Jordi Muñoz and Jose Julio. DIYDrones.com
This library is free software; you can redistribute it and/or This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. version 2.1 of the License, or (at your option) any later version.
Sensor is conected to I2C port Sensor is conected to I2C port
Sensor is initialized in Continuos mode (10Hz) Sensor is initialized in Continuos mode (10Hz)
*/ */
// AVR LibC Includes // AVR LibC Includes
#include <math.h> #include <math.h>
#include "WConstants.h" #include "WConstants.h"
#include <Wire.h> #include <Wire.h>
#include "AP_Compass_HMC5843.h" #include "AP_Compass_HMC5843.h"
#define COMPASS_ADDRESS 0x1E #define COMPASS_ADDRESS 0x1E
#define ConfigRegA 0x00 #define ConfigRegA 0x00
#define ConfigRegB 0x01 #define ConfigRegB 0x01
#define magGain 0x20 #define magGain 0x20
#define PositiveBiasConfig 0x11 #define PositiveBiasConfig 0x11
#define NegativeBiasConfig 0x12 #define NegativeBiasConfig 0x12
#define NormalOperation 0x10 #define NormalOperation 0x10
#define ModeRegister 0x02 #define ModeRegister 0x02
#define ContinuousConversion 0x00 #define ContinuousConversion 0x00
#define SingleConversion 0x01 #define SingleConversion 0x01
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
bool AP_Compass_HMC5843::init() bool AP_Compass_HMC5843::init()
{ {
unsigned long currentTime = millis(); // record current time unsigned long currentTime = millis(); // record current time
int numAttempts = 0; int numAttempts = 0;
int success = 0; int success = 0;
delay(10); delay(10);
// calibration initialisation // calibration initialisation
calibration[0] = 1.0; calibration[0] = 1.0;
calibration[1] = 1.0; calibration[1] = 1.0;
calibration[2] = 1.0; calibration[2] = 1.0;
while( success == 0 && numAttempts < 5 ) while( success == 0 && numAttempts < 5 )
{ {
// record number of attempts at initialisation // record number of attempts at initialisation
numAttempts++; numAttempts++;
// force positiveBias (compass should return 715 for all channels) // force positiveBias (compass should return 715 for all channels)
Wire.beginTransmission(COMPASS_ADDRESS); Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(ConfigRegA); Wire.send(ConfigRegA);
Wire.send(PositiveBiasConfig); Wire.send(PositiveBiasConfig);
if (0 != Wire.endTransmission()) if (0 != Wire.endTransmission())
continue; // compass not responding on the bus continue; // compass not responding on the bus
delay(50); delay(50);
// set gains // set gains
Wire.beginTransmission(COMPASS_ADDRESS); Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(ConfigRegB); Wire.send(ConfigRegB);
Wire.send(magGain); Wire.send(magGain);
Wire.endTransmission(); Wire.endTransmission();
delay(10); delay(10);
Wire.beginTransmission(COMPASS_ADDRESS); Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(ModeRegister); Wire.send(ModeRegister);
Wire.send(SingleConversion); Wire.send(SingleConversion);
Wire.endTransmission(); Wire.endTransmission();
delay(10); delay(10);
// read values from the compass // read values from the compass
read(); read();
delay(10); delay(10);
// calibrate // 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) 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[0] = fabs(715.0 / mag_x);
calibration[1] = fabs(715.0 / mag_y); calibration[1] = fabs(715.0 / mag_y);
calibration[2] = fabs(715.0 / mag_z); calibration[2] = fabs(715.0 / mag_z);
// mark success // mark success
success = 1; success = 1;
} }
// leave test mode // leave test mode
Wire.beginTransmission(COMPASS_ADDRESS); Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(ConfigRegA); Wire.send(ConfigRegA);
Wire.send(NormalOperation); Wire.send(NormalOperation);
Wire.endTransmission(); Wire.endTransmission();
delay(50); delay(50);
Wire.beginTransmission(COMPASS_ADDRESS); Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(ModeRegister); Wire.send(ModeRegister);
Wire.send(ContinuousConversion); // Set continuous mode (default to 10Hz) Wire.send(ContinuousConversion); // Set continuous mode (default to 10Hz)
Wire.endTransmission(); // End transmission Wire.endTransmission(); // End transmission
delay(50); delay(50);
} }
return(success); return(success);
} }
// Read Sensor data // Read Sensor data
void AP_Compass_HMC5843::read() void AP_Compass_HMC5843::read()
{ {
int i = 0; int i = 0;
byte buff[6]; byte buff[6];
Vector3f rot_mag; Vector3f rot_mag;
Wire.beginTransmission(COMPASS_ADDRESS); Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(0x03); //sends address to read from Wire.send(0x03); //sends address to read from
Wire.endTransmission(); //end transmission Wire.endTransmission(); //end transmission
//Wire.beginTransmission(COMPASS_ADDRESS); //Wire.beginTransmission(COMPASS_ADDRESS);
Wire.requestFrom(COMPASS_ADDRESS, 6); // request 6 bytes from device Wire.requestFrom(COMPASS_ADDRESS, 6); // request 6 bytes from device
while(Wire.available()) while(Wire.available())
{ {
buff[i] = Wire.receive(); // receive one byte buff[i] = Wire.receive(); // receive one byte
i++; i++;
} }
Wire.endTransmission(); //end transmission Wire.endTransmission(); //end transmission
if (i==6) // All bytes received? if (i==6) // All bytes received?
{ {
// MSB byte first, then LSB, X,Y,Z // MSB byte first, then LSB, X,Y,Z
mag_x = -((((int)buff[0]) << 8) | buff[1]) * calibration[0]; // X axis 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_y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1]; // Y axis
mag_z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z axis mag_z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2]; // Z axis
last_update = millis(); // record time of update last_update = millis(); // record time of update
// rotate and offset the magnetometer values // rotate and offset the magnetometer values
// XXX this could well be done in common code... // XXX this could well be done in common code...
rot_mag = _orientation_matrix.get() * Vector3f(mag_x,mag_y,mag_z); rot_mag = _orientation_matrix.get() * Vector3f(mag_x,mag_y,mag_z);
rot_mag = rot_mag + _offset.get(); rot_mag = rot_mag + _offset.get();
mag_x = rot_mag.x; mag_x = rot_mag.x;
mag_y = rot_mag.y; mag_y = rot_mag.y;
mag_z = rot_mag.z; mag_z = rot_mag.z;
} }
} }

108
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -1,54 +1,54 @@
#ifndef AP_Compass_HMC5843_H #ifndef AP_Compass_HMC5843_H
#define AP_Compass_HMC5843_H #define AP_Compass_HMC5843_H
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> #include <AP_Math.h>
#include "Compass.h" #include "Compass.h"
// orientations for DIYDrones magnetometer // orientations for DIYDrones magnetometer
#define AP_COMPASS_COMPONENTS_UP_PINS_FORWARD ROTATION_NONE #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_FORWARD_RIGHT ROTATION_YAW_45
#define AP_COMPASS_COMPONENTS_UP_PINS_RIGHT ROTATION_YAW_90 #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_RIGHT ROTATION_YAW_135
#define AP_COMPASS_COMPONENTS_UP_PINS_BACK ROTATION_YAW_180 #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_BACK_LEFT ROTATION_YAW_225
#define AP_COMPASS_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_270 #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_UP_PINS_FORWARD_LEFT ROTATION_YAW_315
#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD ROTATION_ROLL_180 #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_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_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_RIGHT ROTATION_ROLL_180_YAW_135
#define AP_COMPASS_COMPONENTS_DOWN_PINS_BACK ROTATION_PITCH_180 #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_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_LEFT ROTATION_ROLL_180_YAW_270
#define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_315 #define AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_315
// orientations for Sparkfun magnetometer // orientations for Sparkfun magnetometer
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_FORWARD ROTATION_YAW_270 #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_FORWARD_RIGHT ROTATION_YAW_315
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_RIGHT ROTATION_NONE #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_RIGHT ROTATION_YAW_45
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_BACK ROTATION_YAW_90 #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_BACK_LEFT ROTATION_YAW_135
#define AP_COMPASS_SPARKFUN_COMPONENTS_UP_PINS_LEFT ROTATION_YAW_180 #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_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 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_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_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_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 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_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_LEFT ROTATION_ROLL_180
#define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_45 #define AP_COMPASS_SPARKFUN_COMPONENTS_DOWN_PINS_FORWARD_LEFT ROTATION_ROLL_180_YAW_45
class AP_Compass_HMC5843 : public Compass class AP_Compass_HMC5843 : public Compass
{ {
private: private:
float calibration[3]; float calibration[3];
public: public:
AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {} AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
virtual bool init(); virtual bool init();
virtual void read(); virtual void read();
}; };
#endif #endif

242
libraries/AP_Compass/Compass.h
View File

@ -1,121 +1,121 @@
#ifndef Compass_h #ifndef Compass_h
#define Compass_h #define Compass_h
#include <inttypes.h> #include <inttypes.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> #include <AP_Math.h>
// standard rotation matrices // standard rotation matrices
#define ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1) #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_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_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_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_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_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_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_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 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_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_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_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_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_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_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) #define ROTATION_ROLL_180_YAW_315 Matrix3f(0.70710678, -0.70710678, 0, -0.70710678, -0.70710678, 0, 0, 0, -1)
class Compass class Compass
{ {
public: public:
int mag_x; ///< magnetic field strength along the X axis int mag_x; ///< magnetic field strength along the X axis
int mag_y; ///< magnetic field strength along the Y axis int mag_y; ///< magnetic field strength along the Y axis
int mag_z; ///< magnetic field strength along the Z axis int mag_z; ///< magnetic field strength along the Z axis
float heading; ///< compass heading in radians float heading; ///< compass heading in radians
float heading_x; ///< compass vector X magnitude float heading_x; ///< compass vector X magnitude
float heading_y; ///< compass vector Y magnitude float heading_y; ///< compass vector Y magnitude
unsigned long last_update; ///< millis() time of last update unsigned long last_update; ///< millis() time of last update
/// Constructor /// Constructor
/// ///
/// @param key Storage key used for configuration data. /// @param key Storage key used for configuration data.
/// ///
Compass(AP_Var::Key key); Compass(AP_Var::Key key);
/// Initialize the compass device. /// Initialize the compass device.
/// ///
/// @returns True if the compass was initialized OK, false if it was not /// @returns True if the compass was initialized OK, false if it was not
/// found or is not functioning. /// found or is not functioning.
/// ///
virtual bool init(); virtual bool init();
/// Read the compass and update the mag_ variables. /// Read the compass and update the mag_ variables.
/// ///
virtual void read() = 0; virtual void read() = 0;
/// Calculate the tilt-compensated heading_ variables. /// Calculate the tilt-compensated heading_ variables.
/// ///
/// @param roll The current airframe roll angle. /// @param roll The current airframe roll angle.
/// @param pitch The current airframe pitch angle. /// @param pitch The current airframe pitch angle.
/// ///
virtual void calculate(float roll, float pitch); virtual void calculate(float roll, float pitch);
/// Set the compass orientation matrix, used to correct for /// Set the compass orientation matrix, used to correct for
/// various compass mounting positions. /// various compass mounting positions.
/// ///
/// @param rotation_matrix Rotation matrix to transform magnetometer readings /// @param rotation_matrix Rotation matrix to transform magnetometer readings
/// to the body frame. /// to the body frame.
/// ///
virtual void set_orientation(const Matrix3f &rotation_matrix); virtual void set_orientation(const Matrix3f &rotation_matrix);
/// Sets the compass offset x/y/z values. /// Sets the compass offset x/y/z values.
/// ///
/// @param offsets Offsets to the raw mag_ values. /// @param offsets Offsets to the raw mag_ values.
/// ///
virtual void set_offsets(const Vector3f &offsets); virtual void set_offsets(const Vector3f &offsets);
/// Saves the current compass offset x/y/z values. /// Saves the current compass offset x/y/z values.
/// ///
/// This should be invoked periodically to save the offset values maintained by /// This should be invoked periodically to save the offset values maintained by
/// ::null_offsets. /// ::null_offsets.
/// ///
virtual void save_offsets(); virtual void save_offsets();
/// Returns the current offset values /// Returns the current offset values
/// ///
/// @returns The current compass offsets. /// @returns The current compass offsets.
/// ///
virtual Vector3f &get_offsets(); virtual Vector3f &get_offsets();
/// Program new offset values. /// Program new offset values.
/// ///
/// XXX DEPRECATED /// XXX DEPRECATED
/// ///
/// @param x Offset to the raw mag_x value. /// @param x Offset to the raw mag_x value.
/// @param y Offset to the raw mag_y value. /// @param y Offset to the raw mag_y value.
/// @param z Offset to the raw mag_z value. /// @param z Offset to the raw mag_z value.
/// ///
void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); } void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); }
/// Perform automatic offset updates using the results of the DCM matrix. /// Perform automatic offset updates using the results of the DCM matrix.
/// ///
/// @param dcm_matrix The DCM result matrix. /// @param dcm_matrix The DCM result matrix.
/// ///
void null_offsets(const Matrix3f &dcm_matrix); void null_offsets(const Matrix3f &dcm_matrix);
/// Sets the local magnetic field declination. /// Sets the local magnetic field declination.
/// ///
/// @param radians Local field declination. /// @param radians Local field declination.
/// ///
virtual void set_declination(float radians); virtual void set_declination(float radians);
protected: protected:
AP_Var_group _group; ///< storage group holding the compass' calibration data AP_Var_group _group; ///< storage group holding the compass' calibration data
AP_VarS<Matrix3f> _orientation_matrix; AP_VarS<Matrix3f> _orientation_matrix;
AP_VarS<Vector3f> _offset; AP_VarS<Vector3f> _offset;
AP_Float _declination; AP_Float _declination;
bool _null_init_done; ///< first-time-around flag used by offset nulling bool _null_init_done; ///< first-time-around flag used by offset nulling
Matrix3f _last_dcm_matrix; ///< previous DCM matrix used by offset nulling Matrix3f _last_dcm_matrix; ///< previous DCM matrix used by offset nulling
Vector3f _mag_body_last; ///< ?? used by offset nulling Vector3f _mag_body_last; ///< ?? used by offset nulling
}; };
#endif #endif