adafruit-mirror/Adafruit_AHRS
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Refactor to simple AHRS implementation with loose coupling to 10DOF and 9DOF boards.

Browse source
This commit is contained in:
Tony DiCola 2014-08-01 01:34:52 -07:00
parent 3ecc273f7f
commit 3b15ae25ec
20 changed files with 233 additions and 22952 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

38
Adafruit_10DOF.cpp Executable file
View file

@ -0,0 +1,38 @@
#include "Adafruit_10DOF.h"
Adafruit_10DOF::Adafruit_10DOF():
_accel(30301),
_mag(30302),
_gyro(30303),
_bmp(18001)
{}
bool Adafruit_10DOF::begin() {
if (!_accel.begin()) {
return false;
}
if (!_mag.begin()) {
return false;
}
if (!_gyro.begin()) {
return false;
}
if (!_bmp.begin()) {
return false;
}
}
Adafruit_Sensor* Adafruit_10DOF::getSensor(sensors_type_t type) {
switch (type) {
case SENSOR_TYPE_ACCELEROMETER:
return &_accel;
case SENSOR_TYPE_MAGNETIC_FIELD:
return &_mag;
case SENSOR_TYPE_GYROSCOPE:
return &_gyro;
case SENSOR_TYPE_PRESSURE:
return &_bmp;
default:
return NULL;
}
}

26
Adafruit_10DOF.h Executable file
View file

@ -0,0 +1,26 @@
#ifndef __ADAFRUIT_10DOF_H__
#define __ADAFRUIT_10DOF_H__
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include <Adafruit_L3GD20_U.h>
#include <Adafruit_BMP085_U.h>
#include "Adafruit_Sensor_Set.h"
// 10DOF board with an accelerometer, magnetometer, gyroscope, and pressure sensor.
class Adafruit_10DOF: Adafruit_Sensor_Set
{
public:
Adafruit_10DOF();
bool begin();
Adafruit_Sensor* getSensor(sensors_type_t type);
private:
Adafruit_LSM303_Accel_Unified _accel;
Adafruit_LSM303_Mag_Unified _mag;
Adafruit_L3GD20_Unified _gyro;
Adafruit_BMP085_Unified _bmp;
};
#endif

32
Adafruit_9DOF.cpp Executable file
View file

@ -0,0 +1,32 @@
#include "Adafruit_9DOF.h"
Adafruit_9DOF::Adafruit_9DOF():
_accel(30301),
_mag(30302),
_gyro(30303)
{}
bool Adafruit_9DOF::begin() {
if (!_accel.begin()) {
return false;
}
if (!_mag.begin()) {
return false;
}
if (!_gyro.begin()) {
return false;
}
}
Adafruit_Sensor* Adafruit_9DOF::getSensor(sensors_type_t type) {
switch (type) {
case SENSOR_TYPE_ACCELEROMETER:
return &_accel;
case SENSOR_TYPE_MAGNETIC_FIELD:
return &_mag;
case SENSOR_TYPE_GYROSCOPE:
return &_gyro;
default:
return NULL;
}
}

30
Adafruit_9DOF.h Executable file
View file

@ -0,0 +1,30 @@
#ifndef __ADAFRUIT_9DOF_H__
#define __ADAFRUIT_9DOF_H__
#if (ARDUINO >= 100)
#include "Arduino.h"
#else
#include "WProgram.h"
#endif
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
#include <Adafruit_L3GD20_U.h>
#include "Adafruit_Sensor_Set.h"
// 9DOF board with an accelerometer, magnetometer, and gyroscope.
class Adafruit_9DOF: Adafruit_Sensor_Set
{
public:
Adafruit_9DOF();
bool begin();
Adafruit_Sensor* getSensor(sensors_type_t type);
private:
Adafruit_LSM303_Accel_Unified _accel;
Adafruit_LSM303_Mag_Unified _mag;
Adafruit_L3GD20_Unified _gyro;
};
#endif

14
Adafruit_Sensor_Set.h Normal file
View file

@ -0,0 +1,14 @@
#ifndef __ADAFRUIT_SENSOR_SET_H__
#define __ADAFRUIT_SENSOR_SET_H__
#include <Adafruit_Sensor.h>
// Interface for a device that has multiple sensors that can be queried by type.
class Adafruit_Sensor_Set
{
public:
virtual ~Adafruit_Sensor_Set() {}
Adafruit_Sensor* getSensor(sensors_type_t type) { return NULL; }
};
#endif

72
Adafruit_Simple_AHRS.cpp Normal file
View file

@ -0,0 +1,72 @@
#include "Adafruit_Simple_AHRS.h"
// Create a simple AHRS from an explicit accelerometer and magnetometer sensor.
Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(Adafruit_Sensor* accelerometer, Adafruit_Sensor* magnetometer):
_accel(accelerometer),
_mag(magnetometer)
{}
// Create a simple AHRS from a device with multiple sensors.
Adafruit_Simple_AHRS::Adafruit_Simple_AHRS(const Adafruit_Sensor_Set& sensors):
_accel(sensors.getSensor(SENSOR_TYPE_ACCELEROMETER)),
_mag(sensors.getSensor(SENSOR_TYPE_MAGNETIC_FIELD))
{}
// Compute orientation based on accelerometer and magnetometer data.
bool Adafruit_Simple_AHRS::getOrientation(sensors_vec_t* orientation) {
// Validate input and available sensors.
if (orientation == NULL || _accel == NULL || _mag == NULL) return false;
// Grab an acceleromter and magnetometer reading.
sensor_event_t accel_event;
_accel.getEvent(&accel_event);
sensor_event_t mag_event;
_mag.getEvent(&mag_event);
float const PI_F = 3.14159265F;
// roll: Rotation around the X-axis. -180 <= roll <= 180
// a positive roll angle is defined to be a clockwise rotation about the positive X-axis
//
// y
// roll = atan2(---)
// z
//
// where: y, z are returned value from accelerometer sensor
orientation->roll = (float)atan2(accel_event->acceleration.y, accel_event->acceleration.z);
// pitch: Rotation around the Y-axis. -180 <= roll <= 180
// a positive pitch angle is defined to be a clockwise rotation about the positive Y-axis
//
// -x
// pitch = atan(-------------------------------)
// y * sin(roll) + z * cos(roll)
//
// where: x, y, z are returned value from accelerometer sensor
if (accel_event->acceleration.y * sin(orientation->roll) + accel_event->acceleration.z * cos(orientation->roll) == 0)
orientation->pitch = accel_event->acceleration.x > 0 ? (PI_F / 2) : (-PI_F / 2);
else
orientation->pitch = (float)atan(-accel_event->acceleration.x / (accel_event->acceleration.y * sin(orientation->roll) + \
accel_event->acceleration.z * cos(orientation->roll)));
// heading: Rotation around the Z-axis. -180 <= roll <= 180
// a positive heading angle is defined to be a clockwise rotation about the positive Z-axis
//
// z * sin(roll) - y * cos(roll)
// heading = atan2(--------------------------------------------------------------------------)
// x * cos(pitch) + y * sin(pitch) * sin(roll) + z * sin(pitch) * cos(roll))
//
// where: x, y, z are returned value from magnetometer sensor
orientation->heading = (float)atan2(mag_event->magnetic.z * sin(orientation->roll) - mag_event->magnetic.y * cos(orientation->roll), \
mag_event->magnetic.x * cos(orientation->pitch) + \
mag_event->magnetic.y * sin(orientation->pitch) * sin(orientation->roll) + \
mag_event->magnetic.z * sin(orientation->pitch) * cos(orientation->roll));
// Convert angular data to degree
orientation->roll = orientation->roll * 180 / PI_F;
orientation->pitch = orientation->pitch * 180 / PI_F;
orientation->heading = orientation->heading * 180 / PI_F;
return true;
}

21
Adafruit_Simple_AHRS.h Normal file
View file

@ -0,0 +1,21 @@
#ifndef __ADAFRUIT_SIMPLE_AHRS_H__
#define __ADAFRUIT_SIMPLE_AHRS_H__
#include <Adafruit_Sensor.h>
#include "Adafruit_Sensor_Set.h"
// Simple sensor fusion AHRS using an accelerometer and magnetometer.
class Adafruit_Simple_AHRS
{
public:
Adafruit_Simple_AHRS(Adafruit_Sensor* accelerometer, Adafruit_Sensor* magnetometer);
Adafruit_Simple_AHRS(const Adafruit_Sensor_Set& sensors);
bool getOrientation(sensors_vec_t* orientation);
private:
Adafruit_Sensor* _accel;
Adafruit_Sensor* _mag;
};
#endif

159
L3G.cpp
View file

@ -1,159 +0,0 @@
#include "L3G.h"
#include <Wire.h>
#include <math.h>
// Defines ////////////////////////////////////////////////////////////////
// The Arduino two-wire interface uses a 7-bit number for the address,
// and sets the last bit correctly based on reads and writes
#define L3G4200D_ADDRESS_SA0_LOW (0xD0 >> 1)
#define L3G4200D_ADDRESS_SA0_HIGH (0xD2 >> 1)
#define L3GD20_ADDRESS_SA0_LOW (0xD4 >> 1)
#define L3GD20_ADDRESS_SA0_HIGH (0xD6 >> 1)
// Public Methods //////////////////////////////////////////////////////////////
bool L3G::init(byte device, byte sa0)
{
_device = device;
switch (_device)
{
case L3G4200D_DEVICE:
if (sa0 == L3G_SA0_LOW)
{
address = L3G4200D_ADDRESS_SA0_LOW;
return true;
}
else if (sa0 == L3G_SA0_HIGH)
{
address = L3G4200D_ADDRESS_SA0_HIGH;
return true;
}
else
return autoDetectAddress();
break;
case L3GD20_DEVICE:
if (sa0 == L3G_SA0_LOW)
{
address = L3GD20_ADDRESS_SA0_LOW;
return true;
}
else if (sa0 == L3G_SA0_HIGH)
{
address = L3GD20_ADDRESS_SA0_HIGH;
return true;
}
else
return autoDetectAddress();
break;
default:
return autoDetectAddress();
}
}
// Turns on the L3G's gyro and places it in normal mode.
void L3G::enableDefault(void)
{
writeReg(L3G_CTRL_REG1, 0b00001111); // Normal power mode, all axes enabled
writeReg(L3G_CTRL_REG4, 0b00100000); // 2000 dps full scale
}
// Writes a gyro register
void L3G::writeReg(byte reg, byte value)
{
Wire.beginTransmission(address);
Wire.write(reg);
Wire.write(value);
Wire.endTransmission();
}
// Reads a gyro register
byte L3G::readReg(byte reg)
{
byte value;
Wire.beginTransmission(address);
Wire.write(reg);
Wire.endTransmission();
Wire.requestFrom(address, (byte)1);
value = Wire.read();
Wire.endTransmission();
return value;
}
// Reads the 3 gyro channels and stores them in vector g
void L3G::read()
{
measure();
apply_offset();
g = g * (0.07 * 3.14159265 / 180);
}
// Private Methods //////////////////////////////////////////////////////////////
void L3G::measure()
{
Wire.beginTransmission(address);
// assert the MSB of the address to get the gyro
// to do slave-transmit subaddress updating.
Wire.write(L3G_OUT_X_L | (1 << 7));
Wire.endTransmission();
Wire.requestFrom(address, (byte)6);
while (Wire.available() < 6);
uint8_t xlg = Wire.read();
uint8_t xhg = Wire.read();
uint8_t ylg = Wire.read();
uint8_t yhg = Wire.read();
uint8_t zlg = Wire.read();
uint8_t zhg = Wire.read();
// combine high and low bytes
int16_t gx = (int16_t)(xhg << 8 | xlg);
int16_t gy = (int16_t)(yhg << 8 | ylg);
int16_t gz = (int16_t)(zhg << 8 | zlg);
g.x() = gx;
g.y() = gy;
g.z() = gz;
}
void L3G::apply_offset()
{
g = g - gyro_offset;
}
void L3G::measureOffsets()
{
const int sampleCount = 32;
for(int i = 0; i < sampleCount-1; i++)
{
measure();
gyro_offset = gyro_offset + g;
delay(20);
}
gyro_offset = gyro_offset / sampleCount;
}
bool L3G::autoDetectAddress(void)
{
// try each possible address and stop if reading WHO_AM_I returns the expected response
address = L3G4200D_ADDRESS_SA0_LOW;
if (readReg(L3G_WHO_AM_I) == 0xD3) return true;
address = L3G4200D_ADDRESS_SA0_HIGH;
if (readReg(L3G_WHO_AM_I) == 0xD3) return true;
address = L3GD20_ADDRESS_SA0_LOW;
if (readReg(L3G_WHO_AM_I) == 0xD4) return true;
address = L3GD20_ADDRESS_SA0_HIGH;
if (readReg(L3G_WHO_AM_I) == 0xD4) return true;
return false;
}

83
L3G.h
View file

@ -1,83 +0,0 @@
#ifndef L3G_h
#define L3G_h
#include <Arduino.h> // for byte data type
#include "imuMaths.h"
// device types
#define L3G_DEVICE_AUTO 0
#define L3G4200D_DEVICE 1
#define L3GD20_DEVICE 2
// SA0 states
#define L3G_SA0_LOW 0
#define L3G_SA0_HIGH 1
#define L3G_SA0_AUTO 2
// register addresses
#define L3G_WHO_AM_I 0x0F
#define L3G_CTRL_REG1 0x20
#define L3G_CTRL_REG2 0x21
#define L3G_CTRL_REG3 0x22
#define L3G_CTRL_REG4 0x23
#define L3G_CTRL_REG5 0x24
#define L3G_REFERENCE 0x25
#define L3G_OUT_TEMP 0x26
#define L3G_STATUS_REG 0x27
#define L3G_OUT_X_L 0x28
#define L3G_OUT_X_H 0x29
#define L3G_OUT_Y_L 0x2A
#define L3G_OUT_Y_H 0x2B
#define L3G_OUT_Z_L 0x2C
#define L3G_OUT_Z_H 0x2D
#define L3G_FIFO_CTRL_REG 0x2E
#define L3G_FIFO_SRC_REG 0x2F
#define L3G_INT1_CFG 0x30
#define L3G_INT1_SRC 0x31
#define L3G_INT1_THS_XH 0x32
#define L3G_INT1_THS_XL 0x33
#define L3G_INT1_THS_YH 0x34
#define L3G_INT1_THS_YL 0x35
#define L3G_INT1_THS_ZH 0x36
#define L3G_INT1_THS_ZL 0x37
#define L3G_INT1_DURATION 0x38
class L3G
{
public:
imu::Vector g; // gyro angular velocity readings
bool init(byte device = L3G_DEVICE_AUTO, byte sa0 = L3G_SA0_AUTO);
void enableDefault(void);
void writeReg(byte reg, byte value);
byte readReg(byte reg);
void read(void);
void measureOffsets();
private:
imu::Vector gyro_offset;
byte _device; // chip type (4200D or D20)
byte address;
void measure();
void apply_offset();
bool autoDetectAddress(void);
};
#endif

279
LSM303.cpp
View file

@ -1,279 +0,0 @@
#include "LSM303.h"
#include <Wire.h>
#include <math.h>
// Defines ////////////////////////////////////////////////////////////////
// The Arduino two-wire interface uses a 7-bit number for the address,
// and sets the last bit correctly based on reads and writes
#define MAG_ADDRESS (0x3C >> 1)
#define ACC_ADDRESS_SA0_A_LOW (0x30 >> 1)
#define ACC_ADDRESS_SA0_A_HIGH (0x32 >> 1)
// Constructors ////////////////////////////////////////////////////////////////
LSM303::LSM303(void)
{
// These are just some values for a particular unit; it is recommended that
// a calibration be done for your particular unit.
m_max.x() = +540;
m_max.y() = +500;
m_max.z() = 180;
m_min.x() = -520;
m_min.y() = -570;
m_min.z() = -770;
_device = LSM303_DEVICE_AUTO;
acc_address = ACC_ADDRESS_SA0_A_LOW;
io_timeout = 0; // 0 = no timeout
did_timeout = false;
}
// Public Methods //////////////////////////////////////////////////////////////
bool LSM303::timeoutOccurred()
{
return did_timeout;
}
void LSM303::setTimeout(unsigned int timeout)
{
io_timeout = timeout;
}
unsigned int LSM303::getTimeout()
{
return io_timeout;
}
void LSM303::init(byte device, byte sa0_a)
{
_device = device;
switch (_device)
{
case LSM303DLH_DEVICE:
case LSM303DLM_DEVICE:
if (sa0_a == LSM303_SA0_A_LOW)
acc_address = ACC_ADDRESS_SA0_A_LOW;
else if (sa0_a == LSM303_SA0_A_HIGH)
acc_address = ACC_ADDRESS_SA0_A_HIGH;
else
acc_address = (detectSA0_A() == LSM303_SA0_A_HIGH) ? ACC_ADDRESS_SA0_A_HIGH : ACC_ADDRESS_SA0_A_LOW;
break;
case LSM303DLHC_DEVICE:
acc_address = ACC_ADDRESS_SA0_A_HIGH;
break;
default:
// try to auto-detect device
if (detectSA0_A() == LSM303_SA0_A_HIGH)
{
// if device responds on 0011001b (SA0_A is high), assume DLHC
acc_address = ACC_ADDRESS_SA0_A_HIGH;
_device = LSM303DLHC_DEVICE;
}
else
{
// otherwise, assume DLH or DLM (pulled low by default on Pololu boards); query magnetometer WHO_AM_I to differentiate these two
acc_address = ACC_ADDRESS_SA0_A_LOW;
_device = (readMagReg(LSM303_WHO_AM_I_M) == 0x3C) ? LSM303DLM_DEVICE : LSM303DLH_DEVICE;
}
}
}
// Turns on the LSM303's accelerometer and magnetometers and places them in normal
// mode.
void LSM303::enableDefault(void)
{
if (_device == LSM303DLHC_DEVICE)
{
writeAccReg(LSM303_CTRL_REG1_A, 0b01000111); // Normal power mode, all axes enabled, 50 Hz
writeAccReg(LSM303_CTRL_REG4_A, 0x28); // 8 g full scale: FS = 10 on DLHC, high resolution output mode
}
else
{
writeAccReg(LSM303_CTRL_REG1_A, 0b00100111); // normal power mode, all axes enabled, 50 Hz
writeAccReg(LSM303_CTRL_REG4_A, 0b00110000); // 8 g full scale: FS = 11 on DLH, DLM
}
// Enable Magnetometer
// 0x00 = 0b00000000
// Continuous conversion mode
writeMagReg(LSM303_MR_REG_M, 0x00);
}
// Writes an accelerometer register
void LSM303::writeAccReg(byte reg, byte value)
{
Wire.beginTransmission(acc_address);
Wire.write(reg);
Wire.write(value);
last_status = Wire.endTransmission();
}
// Reads an accelerometer register
byte LSM303::readAccReg(byte reg)
{
byte value;
Wire.beginTransmission(acc_address);
Wire.write(reg);
last_status = Wire.endTransmission();
Wire.requestFrom(acc_address, (byte)1);
value = Wire.read();
Wire.endTransmission();
return value;
}
// Writes a magnetometer register
void LSM303::writeMagReg(byte reg, byte value)
{
Wire.beginTransmission(MAG_ADDRESS);
Wire.write(reg);
Wire.write(value);
last_status = Wire.endTransmission();
}
// Reads a magnetometer register
byte LSM303::readMagReg(int reg)
{
byte value;
// if dummy register address (magnetometer Y/Z), use device type to determine actual address
if (reg < 0)
{
switch (reg)
{
case LSM303_OUT_Y_H_M:
reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Y_H_M : LSM303DLM_OUT_Y_H_M;
break;
case LSM303_OUT_Y_L_M:
reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Y_L_M : LSM303DLM_OUT_Y_L_M;
break;
case LSM303_OUT_Z_H_M:
reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Z_H_M : LSM303DLM_OUT_Z_H_M;
break;
case LSM303_OUT_Z_L_M:
reg = (_device == LSM303DLH_DEVICE) ? LSM303DLH_OUT_Z_L_M : LSM303DLM_OUT_Z_L_M;
break;
}
}
Wire.beginTransmission(MAG_ADDRESS);
Wire.write(reg);
last_status = Wire.endTransmission();
Wire.requestFrom(MAG_ADDRESS, 1);
value = Wire.read();
Wire.endTransmission();
return value;
}
void LSM303::setMagGain(magGain value)
{
Wire.beginTransmission(MAG_ADDRESS);
Wire.write(LSM303_CRB_REG_M);
Wire.write((byte) value);
Wire.endTransmission();
}
// Reads the 3 accelerometer channels and stores them in vector a
void LSM303::readAcc(void)
{
Wire.beginTransmission(acc_address);
// assert the MSB of the address to get the accelerometer
// to do slave-transmit subaddress updating.
Wire.write(LSM303_OUT_X_L_A | (1 << 7));
last_status = Wire.endTransmission();
Wire.requestFrom(acc_address, (byte)6);
unsigned int millis_start = millis();
did_timeout = false;
while (Wire.available() < 6) {
if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout) {
did_timeout = true;
return;
}
}
byte xla = Wire.read();
byte xha = Wire.read();
byte yla = Wire.read();
byte yha = Wire.read();
byte zla = Wire.read();
byte zha = Wire.read();
// combine high and low bytes, then shift right to discard lowest 4 bits (which are meaningless)
// GCC performs an arithmetic right shift for signed negative numbers, but this code will not work
// if you port it to a compiler that does a logical right shift instead.
a.x() = ((int16_t)(xha << 8 | xla)) >> 4;
a.y() = ((int16_t)(yha << 8 | yla)) >> 4;
a.z() = ((int16_t)(zha << 8 | zla)) >> 4;
const float accel_scale = 0.0039;
a = a*accel_scale;
}
// Reads the 3 magnetometer channels and stores them in vector m
void LSM303::readMag(void)
{
Wire.beginTransmission(MAG_ADDRESS);
Wire.write(LSM303_OUT_X_H_M);
last_status = Wire.endTransmission();
Wire.requestFrom(MAG_ADDRESS, 6);
unsigned int millis_start = millis();
did_timeout = false;
while (Wire.available() < 6) {
if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout) {
did_timeout = true;
return;
}
}
byte xhm = Wire.read();
byte xlm = Wire.read();
byte yhm, ylm, zhm, zlm;
// DLM, DLHC: register address for Z comes before Y
zhm = Wire.read();
zlm = Wire.read();
yhm = Wire.read();
ylm = Wire.read();
// combine high and low bytes
m.x() = (int16_t)(xhm << 8 | xlm);
m.y() = (int16_t)(yhm << 8 | ylm);
m.z() = (int16_t)(zhm << 8 | zlm);
}
// Reads all 6 channels of the LSM303 and stores them in the object variables
void LSM303::read(void)
{
readAcc();
readMag();
}
// Private Methods //////////////////////////////////////////////////////////////
byte LSM303::detectSA0_A(void)
{
Wire.beginTransmission(ACC_ADDRESS_SA0_A_LOW);
Wire.write(LSM303_CTRL_REG1_A);
last_status = Wire.endTransmission();
Wire.requestFrom(ACC_ADDRESS_SA0_A_LOW, 1);
if (Wire.available())
{
Wire.read();
return LSM303_SA0_A_LOW;
}
else
return LSM303_SA0_A_HIGH;
}

153
LSM303.h
View file

@ -1,153 +0,0 @@
#ifndef LSM303_h
#define LSM303_h
#include <Arduino.h> // for byte data type
#include "imuMaths.h"
// device types
#define LSM303DLH_DEVICE 0
#define LSM303DLM_DEVICE 1
#define LSM303DLHC_DEVICE 2
#define LSM303_DEVICE_AUTO 3
// SA0_A states
#define LSM303_SA0_A_LOW 0
#define LSM303_SA0_A_HIGH 1
#define LSM303_SA0_A_AUTO 2
// register addresses
#define LSM303_CTRL_REG1_A 0x20
#define LSM303_CTRL_REG2_A 0x21
#define LSM303_CTRL_REG3_A 0x22
#define LSM303_CTRL_REG4_A 0x23
#define LSM303_CTRL_REG5_A 0x24
#define LSM303_CTRL_REG6_A 0x25 // DLHC only
#define LSM303_HP_FILTER_RESET_A 0x25 // DLH, DLM only
#define LSM303_REFERENCE_A 0x26
#define LSM303_STATUS_REG_A 0x27
#define LSM303_OUT_X_L_A 0x28
#define LSM303_OUT_X_H_A 0x29
#define LSM303_OUT_Y_L_A 0x2A
#define LSM303_OUT_Y_H_A 0x2B
#define LSM303_OUT_Z_L_A 0x2C
#define LSM303_OUT_Z_H_A 0x2D
#define LSM303_FIFO_CTRL_REG_A 0x2E // DLHC only
#define LSM303_FIFO_SRC_REG_A 0x2F // DLHC only
#define LSM303_INT1_CFG_A 0x30
#define LSM303_INT1_SRC_A 0x31
#define LSM303_INT1_THS_A 0x32
#define LSM303_INT1_DURATION_A 0x33
#define LSM303_INT2_CFG_A 0x34
#define LSM303_INT2_SRC_A 0x35
#define LSM303_INT2_THS_A 0x36
#define LSM303_INT2_DURATION_A 0x37
#define LSM303_CLICK_CFG_A 0x38 // DLHC only
#define LSM303_CLICK_SRC_A 0x39 // DLHC only
#define LSM303_CLICK_THS_A 0x3A // DLHC only
#define LSM303_TIME_LIMIT_A 0x3B // DLHC only
#define LSM303_TIME_LATENCY_A 0x3C // DLHC only
#define LSM303_TIME_WINDOW_A 0x3D // DLHC only
#define LSM303_CRA_REG_M 0x00
#define LSM303_CRB_REG_M 0x01
#define LSM303_MR_REG_M 0x02
#define LSM303_OUT_X_H_M 0x03
#define LSM303_OUT_X_L_M 0x04
#define LSM303_OUT_Y_H_M -1 // The addresses of the Y and Z magnetometer output registers
#define LSM303_OUT_Y_L_M -2 // are reversed on the DLM and DLHC relative to the DLH.
#define LSM303_OUT_Z_H_M -3 // These four defines have dummy values so the library can
#define LSM303_OUT_Z_L_M -4 // determine the correct address based on the device type.
#define LSM303_SR_REG_M 0x09
#define LSM303_IRA_REG_M 0x0A
#define LSM303_IRB_REG_M 0x0B
#define LSM303_IRC_REG_M 0x0C
#define LSM303_WHO_AM_I_M 0x0F // DLM only
#define LSM303_TEMP_OUT_H_M 0x31 // DLHC only
#define LSM303_TEMP_OUT_L_M 0x32 // DLHC only
#define LSM303DLH_OUT_Y_H_M 0x05
#define LSM303DLH_OUT_Y_L_M 0x06
#define LSM303DLH_OUT_Z_H_M 0x07
#define LSM303DLH_OUT_Z_L_M 0x08
#define LSM303DLM_OUT_Z_H_M 0x05
#define LSM303DLM_OUT_Z_L_M 0x06
#define LSM303DLM_OUT_Y_H_M 0x07
#define LSM303DLM_OUT_Y_L_M 0x08
#define LSM303DLHC_OUT_Z_H_M 0x05
#define LSM303DLHC_OUT_Z_L_M 0x06
#define LSM303DLHC_OUT_Y_H_M 0x07
#define LSM303DLHC_OUT_Y_L_M 0x08
class LSM303
{
public:
imu::Vector a; // accelerometer readings
imu::Vector m; // magnetometer readings
imu::Vector m_max; // maximum magnetometer values, used for calibration
imu::Vector m_min; // minimum magnetometer values, used for calibration
byte last_status; // status of last I2C transmission
// HEX = BIN RANGE GAIN X/Y/Z GAIN Z
// DLH (DLM/DLHC) DLH (DLM/DLHC)
// 0x20 = 0b00100000 ±1.3 1055 (1100) 950 (980) (default)
// 0x40 = 0b01000000 ±1.9 795 (855) 710 (760)
// 0x60 = 0b01100000 ±2.5 635 (670) 570 (600)
// 0x80 = 0b10000000 ±4.0 430 (450) 385 (400)
// 0xA0 = 0b10100000 ±4.7 375 (400) 335 (355)
// 0xC0 = 0b11000000 ±5.6 320 (330) 285 (295)
// 0xE0 = 0b11100000 ±8.1 230 (230) 205 (205)
enum magGain { magGain_13 = 0x20, magGain_19 = 0x40, magGain_25 = 0x60, magGain_40 = 0x80,
magGain_47 = 0xA0, magGain_56 = 0xC0, magGain_81 = 0xE0
};
LSM303(void);
void init(byte device = LSM303_DEVICE_AUTO, byte sa0_a = LSM303_SA0_A_AUTO);
byte getDeviceType(void) {
return _device;
}
void enableDefault(void);
void writeAccReg(byte reg, byte value);
byte readAccReg(byte reg);
void writeMagReg(byte reg, byte value);
byte readMagReg(int reg);
void setMagGain(magGain value);
void readAcc(void);
void readMag(void);
void read(void);
void setTimeout(unsigned int timeout);
unsigned int getTimeout(void);
bool timeoutOccurred(void);
private:
byte _device; // chip type (DLH, DLM, or DLHC)
byte acc_address;
unsigned int io_timeout;
bool did_timeout;
byte detectSA0_A(void);
};
#endif

14
README.md
View file

@ -1,14 +0,0 @@
Adafruit_AHRS
=============
AHRS (Altitude and Heading Reference System) example for Adafruit's 9DOF and 10DOF breakouts.
This code is based on the GPL licensed AHRS code from Firetail UAV Systems, available at: http://www.camelsoftware.com/firetail/blog/uncategorized/quaternion-based-ahrs-using-altimu-10-arduino/, which was itself based on the work of David Grayson at http://blog.davidegrayson.com/2012/11/orientation-sensing-with-raspberry-pi.html
Using This Code
===============
This sample code can produce Euler angle, North-East-Down (NED) direction cosine matrix, or Quaternion output using the following commands:
- “output_euler\n” : Switches the output to euler angles (heading, pitch & roll in degrees). This is the default output.
- “output_mat\n” : Switches output to a north-east-down (NED) direction cosine matrix.
- “output_quat\n” : Switches output to quaternion.

358
examples/ahrs/ahrs.pde
View file

@ -1,358 +0,0 @@
/*
AltIMU-10 Quaternion based AHRS
www.camelsoftware.com/firetail
Copyright (C) 2013 Samuel Cowen
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/* Set the baud rate to 115200 to use this sketch, and adjust the
output units by entering one of the following commands in the
Serial Monitor and clicking the "send" button:
"output_euler" - Euler angle output (default value)
"output_mat" - NED cosine matrix output
"output_quat" - Quaternion output
*/
#include <Arduino.h>
#include <Wire.h>
#include <EEPROM.h>
#include "L3G.h"
#include "LSM303.h"
#include "imuMaths.h"
#define OUTPUT_EULER 0 // heading, pitch, roll
#define OUTPUT_MATRIX 1 // a north-east-down rotation matrix
#define OUTPUT_QUATERNION 2 // a quaternion!
L3G l3g;
LSM303 lsm303;
imu::Quaternion rotation_buf;
imu::Quaternion offset;
long delta_t;
bool warmup = true;
long start_millis;
//float alt = 0;
//float temp = 0;
int output_mode = OUTPUT_EULER;
bool auto_output = true;
int mag_cal_counter;
int write_offset(imu::Quaternion* value)
{
const byte* p = (const byte*)value;
int i;
for (i = 0; i < sizeof(imu::Quaternion); i++)
EEPROM.write(i, *p++);
return i;
}
int read_offset(imu::Quaternion* value)
{
byte* p = (byte*)value;
int i;
for (i = 0; i < sizeof(imu::Quaternion); i++)
*p++ = EEPROM.read(i);
return i;
}
int write_mag_cal(imu::Vector min, imu::Vector max)
{
byte* p = (byte*)&min;
int i;
int ret;
int pos = sizeof(imu::Quaternion);
for(i = pos; i < pos + sizeof(imu::Vector); i++)
EEPROM.write(i, *p++);
ret = i;
p = (byte*)&max;
pos = sizeof(imu::Quaternion) + sizeof(imu::Vector);
for(i = pos; i < pos + sizeof(imu::Vector); i++)
EEPROM.write(i, *p++);
return i + ret;
}
int read_mag_cal(imu::Vector* min, imu::Vector* max)
{
byte* p = (byte*)min;
int i;
int ret;
int pos = sizeof(imu::Quaternion);
for(i = pos; i < pos + sizeof(imu::Vector); i++)
*p++ = EEPROM.read(i);
ret = i;
p = (byte*)&max;
pos = sizeof(imu::Quaternion) + sizeof(imu::Vector);
for(i = pos; i < pos + sizeof(imu::Vector); i++)
*p++ = EEPROM.read(i);
return i + ret;
}
void output(imu::Quaternion rotation)
{
switch(output_mode)
{
case OUTPUT_EULER:
{
imu::Vector euler;
euler = rotation.toEuler();
euler.toDegrees();
Serial.print(F("euler: "));
Serial.print(euler.x());
Serial.print(F(" "));
Serial.print(euler.y());
Serial.print(F(" "));
Serial.println(euler.z());
}
break;
case OUTPUT_MATRIX:
{
imu::Matrix m = rotation.toMatrix();
Serial.print(F("matrix: "));
int i, j;
for(i = 0; i <= 2; i++)
{
for(j = 0; j <= 2; j++)
{
Serial.print(m.cell(i, j)); Serial.print(" ");
}
}
Serial.print(F("\n"));
}
break;
case OUTPUT_QUATERNION:
{
Serial.print(F("quat: "));
Serial.print(rotation.w());
Serial.print(F(" "));
Serial.print(rotation.x());
Serial.print(F(" "));
Serial.print(rotation.y());
Serial.print(F(" "));
Serial.println(rotation.z());
}
break;
}
//Serial.print("alt: ");
//Serial.println(alt);
//Serial.print("temp: ");
//Serial.println(temp);
}
void setup()
{
Serial.begin(115200);
Serial.setTimeout(1);
Serial.println(F("Adafruit 9/10DOF AHRS"));
Serial.println(F("www.CamelSoftware.com"));
delta_t = millis();
Wire.begin();
l3g.init();
l3g.enableDefault();
l3g.measureOffsets();
lsm303.init();
lsm303.enableDefault();
//lps331.init();
//lps331.enableDefault();
Serial.println(F("Done"));
mag_cal_counter = 0;
start_millis = millis();
//read_offset(&offset);
if(abs(offset.magnitude() - 1) >= 0.1 || isnan(offset.magnitude())) //if installation offset isn't a valid unit quaternion
{
imu::Quaternion qz; //set it to a unit quaternion with no rotation (no offset)
offset = qz;
}
//read_mag_cal(&lsm303.m_min, &lsm303.m_max);
imu::Vector acc;
imu::Vector mag;
lsm303.readAcc();
lsm303.readMag();
acc = lsm303.a;
mag = lsm303.m;
imu::Vector down = acc.invert();
imu::Vector east = down.cross(mag);
imu::Vector north = east.cross(down);
down.normalize();
east.normalize();
north.normalize();
imu::Matrix m;
m.vector_to_row(north, 0);
m.vector_to_row(east, 1);
m.vector_to_row(down, 2);
rotation_buf.fromMatrix(m);
}
void loop()
{
double dt = millis() - delta_t;
if(dt < 20)
return;
dt /= 1000;
if(warmup)
{
if((millis() - start_millis) > 200)
{
warmup = false;
}
}
delta_t = millis();
imu::Vector ang_vel;
imu::Vector acc;
imu::Vector mag;
l3g.read();
ang_vel = l3g.g;
lsm303.readAcc();
lsm303.readMag();
acc = lsm303.a;
mag = lsm303.m;
if(mag_cal_counter) //if a mag cal is underway
{
mag_cal_counter++; //increment counter
if(mag_cal_counter > 3000) //if counter has reached 3000, mag cal has been running for 60 seconds
{
mag_cal_counter = 0; //stop the mag cal
write_mag_cal(lsm303.m_min, lsm303.m_max);
}
if(mag.x() > lsm303.m_max.x())
lsm303.m_max.x() = mag.x();
if(mag.y() > lsm303.m_max.y())
lsm303.m_max.y() = mag.y();
if(mag.z() > lsm303.m_max.z())
lsm303.m_max.z() = mag.z();
if(mag.x() < lsm303.m_min.x())
lsm303.m_max.x() = mag.x();
if(mag.y() < lsm303.m_min.y())
lsm303.m_max.y() = mag.y();
if(mag.z() < lsm303.m_min.z())
lsm303.m_max.z() = mag.z();
}
imu::Vector correction;
if(abs(acc.magnitude()-1) <= 0.15)
{
float correction_strength = 0.0005;
if(warmup)
correction_strength = 0.01;
imu::Vector down = acc.invert();
imu::Vector east = down.cross(mag);
imu::Vector north = east.cross(down);
down.normalize();
east.normalize();
north.normalize();
imu::Matrix rotationMatrix = rotation_buf.toMatrix();
correction = (
north.cross(rotationMatrix.row_to_vector(0)) +
east.cross(rotationMatrix.row_to_vector(1)) +
down.cross(rotationMatrix.row_to_vector(2))
) * correction_strength;
}
imu::Vector w;
w = ang_vel + correction;
imu::Quaternion q(1, w.x()*dt/2.0, w.y()*dt/2.0, w.z()*dt/2.0);
rotation_buf = rotation_buf*q;
rotation_buf.normalize();
imu::Quaternion rotation = offset.conjugate()*rotation_buf;
rotation.normalize();
//alt = (0.1 * lps331.pressureToAltitudeFeet(lps331.readPressureInchesHg())) + (0.9) * alt;
//temp = (0.1 * lps331.readTemperatureC()) + (0.9) * temp;
if(auto_output)
output(rotation);
char buffer[20];
memset(buffer, '\0', 20);
Serial.readBytesUntil('\n', buffer, 20);
if(strncmp(buffer, "output_euler", 12) == 0)
output_mode= OUTPUT_EULER;
if(strncmp(buffer, "output_mat", 11) == 0)
output_mode = OUTPUT_MATRIX;
if(strncmp(buffer, "output_quat", 12) == 0)
output_mode = OUTPUT_QUATERNION;
if(strncmp(buffer, "auto_on", 7) == 0)
auto_output = true;
if(strncmp(buffer, "auto_off", 8) == 0)
auto_output = false;
if(strncmp(buffer, "read", 4) == 0)
output(rotation);
if(strncmp(buffer, "set_offset", 6) == 0)
{
offset = rotation_buf;
write_offset(&offset);
}
if(strncmp(buffer, "mag_cal", 7) == 0)
{
mag_cal_counter = 1;
lsm303.m_max = imu::Vector();
lsm303.m_min = imu::Vector();
}
}

BIN
examples/ahrs/processing/OBJLoader.zip
View file

Binary file not shown.

97
examples/ahrs/processing/cuberotate.pde
View file

@ -1,97 +0,0 @@
import processing.serial.*;
import java.awt.datatransfer.*;
import java.awt.Toolkit;
import processing.opengl.*;
import saito.objloader.*;
float roll = 0.0F; // Drawn along X axis in Processing
float pitch = 0.0F; // Drawn along Z axis in Processing
float yaw = 0.0F; // Drawn along Y axis in Processing
OBJModel model;
Serial port;
String buffer = "";
void setup()
{
size( 400, 400, OPENGL);
frameRate(30);
model = new OBJModel(this);
model.load("airplane.obj");
model.scale(3.5);
// ToDo: Check for errors, this will fail with no serial device present
String ttyPort = Serial.list()[0];
port = new Serial(this, ttyPort, 115200);
port.bufferUntil('\n');
}
void draw()
{
background(128, 128, 128);
// Set a new co-ordinate space
pushMatrix();
// Get some light in here
lights();
// Displace objects from 0,0
translate(200, 200, 0);
// Rotate shapes around the X/Y/Z axis (values in radians, 0..Pi*2)
rotateX(radians(roll));
rotateZ(radians(pitch));
rotateY(radians(yaw));
pushMatrix();
rotateY(PI/2);
noStroke();
model.draw();
popMatrix();
// Roll Axis (X in Processing)
stroke(255, 0, 0);
box(250, 1, 1);
text("Roll (X)", 85, -10, 0);
// Pitch Axis (Z in Processing)
stroke(0, 255, 0);
box(1, 1, 200);
text("Pitch (Y)", -25, -10, 100);
// Yaw Axis (Y in Processing)
stroke(0, 0, 255);
box(1, 50, 1);
text("Yaw (Z)", -25, -35, 0);
// Render legend in normal XYZ space
popMatrix();
stroke(255);
text("Roll (X):", 10, 20);
text(roll, 75, 20);
text("Pitch (Y):", 10, 35);
text(pitch, 75, 35);
text("Yaw (Z):", 10, 50);
text(yaw, 75, 50);
// Render incoming text from the Arduino sketch
text("Incoming Data: " + buffer, 10, 385);
}
void serialEvent(Serial p)
{
String incoming = p.readString();
if ((incoming.length() > 8))
{
String[] list = split(incoming, " ");
if ( (list.length > 0) && (list[0].equals("euler:")) )
{
roll = float(list[1]);
pitch = float(list[2]);
yaw = float(list[3]);
buffer = incoming;
}
}
}

15
examples/ahrs/processing/data/airplane.mtl
View file

@ -1,15 +0,0 @@
#
## Alias OBJ Material File
# Exported from SketchUp, (c) 2000-2012 Trimble Navigation Limited
newmtl airplane_01_Body1
Ka 0.000000 0.000000 0.000000
Kd 0.254902 0.278431 0.286275
Ks 0.330000 0.330000 0.330000
map_Kd airplane/airplane_01_Body1.jpg
newmtl FrontColor
Ka 0.000000 0.000000 0.000000
Kd 1.000000 1.000000 1.000000
Ks 0.330000 0.330000 0.330000

21275
examples/ahrs/processing/data/airplane.obj
View file

File diff suppressed because it is too large Load diff

BIN
examples/ahrs/processing/data/airplane/airplane_01_Body1.jpg
View file

Binary file not shown.
Side by side

Before

Width:  |  Height:  |  Size: 504 KiB

384
imuMaths.cpp
View file

@ -1,384 +0,0 @@
/*
AltIMU-10 Quaternion based AHRS
www.camelsoftware.com/firetail
Copyright (C) 2013 Samuel Cowen
The latest version of the IMU Maths library will always be available at
http://sourceforge.net/p/firetail/code/ci/master/tree/
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "imuMaths.h"
using namespace imu;
double max(double a, double b)
{
if(a >= b)
return a;
return b;
}
Vector::Vector()
{
_x = _y = _z = 0;
}
Vector::Vector(double a, double b, double c)
{
_x = a;
_y = b;
_z = c;
}
double Vector::magnitude()
{
double res = (_x*_x) + (_y*_y) + (_z*_z);
if((res > 1.001) || (res < 0.999))
return sqrt(res);
return 1;
}
void Vector::normalize()
{
double mag = magnitude();
if(mag != 0)
{
_x = _x/mag;
_y = _y/mag;
_z = _z/mag;
}
}
Vector Vector::cross(Vector vec)
{
Vector ret;
ret._x = (_y * vec._z) - (_z * vec._y);
ret._y = (_z * vec._x) - (_x * vec._z);
ret._z = (_x * vec._y) - (_y * vec._x);
return ret;
}
Vector Vector::scale(double scalar)
{
Vector ret;
ret.x() = _x * scalar;
ret.y() = _y * scalar;
ret.z() = _z * scalar;
return ret;
}
void Vector::toDegrees()
{
_x *= 180/M_PI;
_y *= 180/M_PI;
_z *= 180/M_PI;
}
void Vector::toRadians()
{
_x *= M_PI/180;
_y *= M_PI/180;
_z *= M_PI/180;
}
double Vector::dot(Vector vec)
{
return (_x * vec._x) + (_y * vec._y) + (_z * vec._z);
}
Vector Vector::operator + (Vector vec)
{
Vector ret;
ret.x() = _x + vec._x;
ret.y() = _y + vec._y;
ret.z() = _z + vec._z;
return ret;
}
Vector Vector::operator - (Vector vec)
{
Vector ret;
ret.x() = _x - vec._x;
ret.y() = _y - vec._y;
ret.z() = _z - vec._z;
return ret;
}
Vector Vector::operator * (double scalar)
{
return scale(scalar);
}
Vector Vector::operator / (double scalar)
{
Vector ret;
ret.x() = _x / scalar;
ret.y() = _y / scalar;
ret.z() = _z / scalar;
return ret;
}
Matrix::Matrix()
{
uint8_t i;
uint8_t j;
for(i = 0; i <= 2; i++)
for(j = 0; j <= 2; j++)
{
if(i == j)
_cell[i][j] = 1.0;
else
_cell[i][j] = 0;
}
}
Matrix Matrix::operator + (Matrix m)
{
Matrix ret;
uint8_t i;
uint8_t j;
for(i = 0; i <= 2; i++)
for(j = 0; j <= 2; j++)
ret._cell[i][j] = _cell[i][j] + m._cell[i][j];
return ret;
}
Matrix Matrix::operator - (Matrix m)
{
Matrix ret;
uint8_t i;
uint8_t j;
for(i = 0; i <= 2; i++)
for(j = 0; j <= 2; j++)
ret._cell[i][j] = _cell[i][j] - m._cell[i][j];
return ret;
}
Matrix Matrix::operator * (double scalar)
{
Matrix ret;
uint8_t i;
uint8_t j;
for(i = 0; i <= 2; i++)
for(j = 0; j <= 2; j++)
ret._cell[i][j] = _cell[i][j] * scalar;
return ret;
}
Matrix Matrix::operator * (Matrix m)
{
Matrix ret;
uint8_t i;
for(i = 0; i <= 2; i++)
{
uint8_t j;
for(j = 0; j <= 2; j++)
{
//so for each cell in the output
Vector row = row_to_vector(i);
Vector column = m.col_to_vector(j);
ret._cell[i][j] = row.dot(column);
}
}
return ret;
}
Vector Matrix::col_to_vector(uint8_t rn)
{
Vector ret;
ret.x() = _cell[0][rn];
ret.y() = _cell[1][rn];
ret.z() = _cell[2][rn];
return ret;
}
Vector Matrix::row_to_vector(uint8_t rn)
{
Vector ret;
ret.x() = _cell[rn][0];
ret.y() = _cell[rn][1];
ret.z() = _cell[rn][2];
return ret;
}
void Matrix::vector_to_row(Vector v, int row)
{
_cell[row][0] = v.x();
_cell[row][1] = v.y();
_cell[row][2] = v.z();
}
double& Matrix::operator ()(int row, int col)
{
return _cell[row][col];
}
double& Matrix::cell(int row, int col)
{
return _cell[row][col];
}
Quaternion::Quaternion()
{
_w = 1.0;
_x = _y = _z = 0.0;
}
Quaternion::Quaternion(double iw, double i_x, double i_y, double i_z)
{
_w = iw;
_x = i_x;
_y = i_y;
_z = i_z;
}
Quaternion::Quaternion(double iw, Vector vec)
{
_w = iw;
_x = vec.x();
_y = vec.y();
_z = vec.z();
}
double Quaternion::magnitude()
{
double res = (_w*_w) + (_x*_x) + (_y*_y) + (_z*_z);
if((res > 1.001) || (res < 0.999)) //only normalise if it needs to be done - saves a call to sqrt()
return sqrt(res);
return 1;
}
void Quaternion::normalize()
{
double mag = magnitude();
if(mag == 0) //avoid divide b_y _zero
return;
_w = _w/mag;
_x = _x/mag;
_y = _y/mag;
_z = _z/mag;
}
Quaternion Quaternion::operator * (Quaternion q)
{
Quaternion ret;
ret._w = ((_w*q._w) - (_x*q._x) - (_y*q._y) - (_z*q._z));
ret._x = ((_w*q._x) + (_x*q._w) + (_y*q._z) - (_z*q._y));
ret._y = ((_w*q._y) - (_x*q._z) + (_y*q._w) + (_z*q._x));
ret._z = ((_w*q._z) + (_x*q._y) - (_y*q._x) + (_z*q._w));
return ret;
}
void Quaternion::fromMatrix(Matrix m)
{
float tr = m(0, 0) + m(1, 1) + m(2, 2);
float S = 0.0;
if (tr > 0)
{
S = sqrt(tr+1.0) * 2;
_w = 0.25 * S;
_x = (m(2, 1) - m(1, 2)) / S;
_y = (m(0, 2) - m(2, 0)) / S;
_z = (m(1, 0) - m(0, 1)) / S;
}
else if ((m(0, 0) < m(1, 1))&(m(0, 0) < m(2, 2)))
{
S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;
_w = (m(2, 1) - m(1, 2)) / S;
_x = 0.25 * S;
_y = (m(0, 1) + m(1, 0)) / S;
_z = (m(0, 2) + m(2, 0)) / S;
}
else if (m(1, 1) < m(2, 2))
{
S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
_w = (m(0, 2) - m(2, 0)) / S;
_x = (m(0, 1) + m(1, 0)) / S;
_y = 0.25 * S;
_z = (m(1, 2) + m(2, 1)) / S;
}
else
{
S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2;
_w = (m(1, 0) - m(0, 1)) / S;
_x = (m(0, 2) + m(2, 0)) / S;
_y = (m(1, 2) + m(2, 1)) / S;
_z = 0.25 * S;
}
}
Matrix Quaternion::toMatrix()
{
Matrix ret;
ret.cell(0, 0) = 1-(2*(_y*_y))-(2*(_z*_z));
ret.cell(0, 1) = (2*_x*_y)-(2*_w*_z);
ret.cell(0, 2) = (2*_x*_z)+(2*_w*_y);
ret.cell(1, 0) = (2*_x*_y)+(2*_w*_z);
ret.cell(1, 1) = 1-(2*(_x*_x))-(2*(_z*_z));
ret.cell(1, 2) = (2*(_y*_z))-(2*(_w*_x));
ret.cell(2, 0) = (2*(_x*_z))-(2*_w*_y);
ret.cell(2, 1) = (2*_y*_z)+(2*_w*_x);
ret.cell(2, 2) = 1-(2*(_x*_x))-(2*(_y*_y));
return ret;
}
Vector Quaternion::toEuler()
{
Vector ret;
double sqw = _w*_w;
double sqx = _x*_x;
double sqy = _y*_y;
double sqz = _z*_z;
ret.x() = atan2(2.0*(_x*_y+_z*_w),(sqx-sqy-sqz+sqw));
ret.y() = asin(-2.0*(_x*_z-_y*_w)/(sqx+sqy+sqz+sqw));
ret.z() = atan2(2.0*(_y*_z+_x*_w),(-sqx-sqy+sqz+sqw));
return ret;
}
Quaternion Quaternion::conjugate()
{
Quaternion q;
q.w() = _w;
q.x() = -_x;
q.y() = -_y;
q.z() = -_z;
return q;
}

135
imuMaths.h
View file

@ -1,135 +0,0 @@
/*
AltIMU-10 Quaternion based AHRS
www.camelsoftware.com/firetail
Copyright (C) 2013 Samuel Cowen
The latest version of the IMU Maths library will always be available at
http://sourceforge.net/p/firetail/code/ci/master/tree/
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef IMUMATH_H
#define IMUMATH_H
#include <math.h>
#include <stdint.h>
namespace imu
{
class Vector
{
public:
Vector();
Vector(double _x, double _y, double _z);
double magnitude(); //return the magnitude of the vector
void normalize(); //normalise the vector
Vector cross(Vector vec);
double dot(Vector vec);
Vector scale(double scalar);
void toDegrees();
void toRadians();
double &x() {
return _x;
}
double &y() {
return _y;
}
double &z() {
return _z;
}
Vector operator + (Vector);
Vector operator - (Vector);
Vector invert() {
return Vector(-_x, -_y, -_z);
}
Vector operator * (double scalar);
Vector operator / (double scalar);
private:
double _x, _y, _z;
};
class Matrix
{
public:
Matrix();
Matrix operator + (Matrix);
Matrix operator - (Matrix);
Matrix operator * (double scalar);
Matrix operator * (Matrix m);
double& operator ()(int row, int col);
double& cell(int row, int col);
Vector row_to_vector(uint8_t rn);
Vector col_to_vector(uint8_t cn);
void vector_to_row(Vector v, int row);
protected:
double _cell[3][3]; //x, y
};
class Quaternion
{
public:
Quaternion();
Quaternion(double iw, double ix, double iy, double iz);
Quaternion(double w, Vector vec);
double& w() {
return _w;
}
double& x() {
return _x;
}
double& y() {
return _y;
}
double& z() {
return _z;
}
double magnitude();
void normalize();
Quaternion conjugate();
void fromMatrix(Matrix m);
Matrix toMatrix();
Vector toEuler();
Quaternion operator * (Quaternion q);
private:
double _w, _x, _y, _z;
};
};
#endif