Kris Reynolds
AHRS library for the Polulu minIMU-9 Ability to interface with the Polulu Python minIMU-9 monitor
Revision 1:3272ece36ce1, committed 2012-04-23
- Comitter:
- krmreynolds
- Date:
- Mon Apr 23 14:31:08 2012 +0000
- Parent:
- 0:dc35364e2291
- Commit message:
- Ported L3G4200D and LSM303 from the Polulu website, dropped the horrible code that someone else built
Changed in this revision
diff -r dc35364e2291 -r 3272ece36ce1 L3G4200D.cpp
--- a/L3G4200D.cpp Thu Apr 12 13:47:23 2012 +0000
+++ b/L3G4200D.cpp Mon Apr 23 14:31:08 2012 +0000
@@ -1,5 +1,5 @@
/* mbed L3G4200D Library version 0.1
- * Copyright (c) 2012 Prediluted
+ * Copyright (c) 2012 Ported to MBED and modified by Prediluted (Kris Reynolds)
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
@@ -20,116 +20,52 @@
* THE SOFTWARE.
*/
#include <L3G4200D.h>
+#include <math.h>
+#include "mbed.h"
-// L3G4200D I2C address
-const int L3G4200D::GYR_ADDRESS = 0xd2;
-// L3G4200D register addresses
-const int L3G4200D::WHO_AM_I = 0x0f;
-const int L3G4200D::CTRL_REG1 = 0x20;
-const int L3G4200D::CTRL_REG2 = 0x21;
-const int L3G4200D::CTRL_REG3 = 0x22;
-const int L3G4200D::CTRL_REG4 = 0x23;
-const int L3G4200D::CTRL_REG5 = 0x24;
-const int L3G4200D::REFERENCE = 0x25;
-const int L3G4200D::OUT_TEMP = 0x26;
-const int L3G4200D::STATUS_REG = 0x27;
-const int L3G4200D::OUT_X_L = 0x28;
-const int L3G4200D::OUT_X_H = 0x29;
-const int L3G4200D::OUT_Y_L = 0x2a;
-const int L3G4200D::OUT_Y_H = 0x2b;
-const int L3G4200D::OUT_Z_L = 0x2c;
-const int L3G4200D::OUT_Z_H = 0x2d;
-const int L3G4200D::FIFO_CTRL_REG = 0x2e;
-const int L3G4200D::FIFO_SRC_REG = 0x2f;
-const int L3G4200D::INT1_CFG = 0x30;
-const int L3G4200D::INT1_SRC = 0x31;
-const int L3G4200D::INT1_THS_XH = 0x32;
-const int L3G4200D::INT1_THS_XL = 0x33;
-const int L3G4200D::INT1_THS_YH = 0x34;
-const int L3G4200D::INT1_THS_YL = 0x35;
-const int L3G4200D::INT1_THS_ZH = 0x36;
-const int L3G4200D::INT1_THS_ZL = 0x37;
-const int L3G4200D::INT1_DURATION = 0x38;
+
+L3G4200D::L3G4200D( PinName sda = p9, PinName scl = p10 ) : i2c( sda, scl ) {
+ writeReg( L3G4200D_CTRL_REG1, 0x0f );
+}
-// -----------------------------------------------
-L3G4200D::L3G4200D( PinName sda = p9, PinName scl = p10 ) : _i2c( sda, scl ) {
+void L3G4200D::writeReg( byte reg, byte value ) {
+ char data[2] = { reg, value };
+ i2c.write( GYR_ADDRESS, data, 2 );
+}
- // Check that you're talking with an L3G4200D device
- if ( this->registerRead( WHO_AM_I ) == 0xd3 ) {
- _status = 0;
- } else {
- _status = 1;
- return;
- }
-
- // Enable normal mode...
- this->registerWrite( CTRL_REG1, 0x0f );
-
+char L3G4200D::readReg( byte reg ) {
+ char value[1];
+ char data[1] = { reg };
+ i2c.write( GYR_ADDRESS, data, 1 );
+ i2c.read( GYR_ADDRESS, value, 1 );
+ return value[0];
}
-L3G4200D::L3G4200D( ) : _i2c( p9, p10 ) {};
+void L3G4200D::read( void ) {
+ char data[1] = { L3G4200D_OUT_X_L | (1<<7) };
+ char output[6];
+ i2c.write( GYR_ADDRESS, data, 1 );
-// -----------------------------------------------
-int L3G4200D::registerRead( int reg ) {
- _bytes[0] = reg & 0xff;
- _status = _i2c.write( GYR_ADDRESS, _bytes, 1 );
- if ( _status == 0 ) {
- _status = _i2c.read( GYR_ADDRESS + 1, _bytes, 1 );
- return( _bytes[0] );
- }
- return( 0 );
-}
+ i2c.read( GYR_ADDRESS, output, 6 );
-// -----------------------------------------------
-void L3G4200D::registerWrite( int reg, char data ) {
- _bytes[0] = reg & 0xff;
- _bytes[1] = data & 0xff;
- _status = _i2c.write( GYR_ADDRESS, _bytes, 2 );
+ g.x = short( output[1] << 8 | output[0] );
+ g.y = short( output[3] << 8 | output[2] );
+ g.z = short( output[5] << 8 | output[4] );
}
-// -----------------------------------------------
-std::vector<short> L3G4200D::read( void ) {
- std::vector<short> gyr( 3, 0 );
- _bytes[0] = OUT_X_L | (1<<7);
- _status = _i2c.write( GYR_ADDRESS, _bytes, 1 );
- if ( _status == 0 ) {
- _status = _i2c.read( GYR_ADDRESS + 1, _bytes, 6 );
- if ( _status == 0 ) {
- for ( int i=0; i<3; i++ ) {
- gyr[i] = short( _bytes[2*i] | ( _bytes[2*i+1] << 8 ));
- }
- }
- }
-
- return( gyr );
+void L3G4200D::vector_cross(const Plane *a,const Plane *b, Plane *out) {
+ out->x = a->y*b->z - a->z*b->y;
+ out->y = a->z*b->x - a->x*b->z;
+ out->z = a->x*b->y - a->y*b->x;
}
-// -----------------------------------------------
-std::vector<float> L3G4200D::angularVelocity( void ) {
-
- std::vector<float> angv(3, 0);
- const float fs[] = { 250., 500., 2000., 2000. };
- float fullscale = fs[ ( this->registerRead( L3G4200D::CTRL_REG4 ) >> 4 ) & 0x3 ];
- std::vector<short> g = this->read();
- if ( _status == 0 ) {
- for ( int i=0; i<3; i++ ) {
- angv[i] = float( g[i] ) / 32768. * fullscale;
- }
- }
- return( angv );
+float L3G4200D::vector_dot(const Plane *a,const Plane *b) {
+ return a->x*b->x+a->y*b->y+a->z*b->z;
}
-
-// -----------------------------------------------
-int L3G4200D::temperature( void ) {
-
- _bytes[0] = OUT_TEMP;
- _status = _i2c.write( GYR_ADDRESS, _bytes, 1 );
- if ( _status == 0 ) {
- _status = _i2c.read( GYR_ADDRESS + 1, _bytes, 1 );
- if ( _status == 0 ) {
- return( int( _bytes[0] ) );
- }
- }
- return( 0 );
+void L3G4200D::vector_normalize(Plane *a) {
+ float mag = sqrt(vector_dot(a,a));
+ a->x /= mag;
+ a->y /= mag;
+ a->z /= mag;
}
\ No newline at end of file
diff -r dc35364e2291 -r 3272ece36ce1 L3G4200D.h
--- a/L3G4200D.h Thu Apr 12 13:47:23 2012 +0000
+++ b/L3G4200D.h Mon Apr 23 14:31:08 2012 +0000
@@ -1,112 +1,62 @@
-/* mbed L3G4200D Library version 0.1
- * Copyright (c) 2012 Prediluted
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
#ifndef L3G4200D_h
#define L3G4200D_h
#include "mbed.h"
-#include <vector>
+// register addresses
+
+#define L3G4200D_WHO_AM_I 0x0F
+
+#define L3G4200D_CTRL_REG1 0x20
+#define L3G4200D_CTRL_REG2 0x21
+#define L3G4200D_CTRL_REG3 0x22
+#define L3G4200D_CTRL_REG4 0x23
+#define L3G4200D_CTRL_REG5 0x24
+#define L3G4200D_REFERENCE 0x25
+#define L3G4200D_OUT_TEMP 0x26
+#define L3G4200D_STATUS_REG 0x27
+
+#define L3G4200D_OUT_X_L 0x28
+#define L3G4200D_OUT_X_H 0x29
+#define L3G4200D_OUT_Y_L 0x2A
+#define L3G4200D_OUT_Y_H 0x2B
+#define L3G4200D_OUT_Z_L 0x2C
+#define L3G4200D_OUT_Z_H 0x2D
+
+#define L3G4200D_FIFO_CTRL_REG 0x2E
+#define L3G4200D_FIFO_SRC_REG 0x2F
+
+#define L3G4200D_INT1_CFG 0x30
+#define L3G4200D_INT1_SRC 0x31
+#define L3G4200D_INT1_THS_XH 0x32
+#define L3G4200D_INT1_THS_XL 0x33
+#define L3G4200D_INT1_THS_YH 0x34
+#define L3G4200D_INT1_THS_YL 0x35
+#define L3G4200D_INT1_THS_ZH 0x36
+#define L3G4200D_INT1_THS_ZL 0x37
+#define L3G4200D_INT1_DURATION 0x38
+#define GYR_ADDRESS ( 0xD2 >> 1 )
+
+typedef unsigned char byte;
class L3G4200D {
-
public:
+ typedef struct Plane {
+ float x, y, z;
+ } Plane;
- /**
- * Create an L3G4200D object connected to the specified I2C pins
- * @param sda I2C SDA pin
- * @param scl I2C SCL pin
- */
- L3G4200D( PinName sda, PinName scl );
- L3G4200D( void );
-
- /**
- * Return status code of prevoius function call
- */
- inline int getStatus( void ) { return( _status ); }
-
- /**
- * Read specified register content
- * @param reg register address
- */
- int registerRead( int reg );
-
- /**
- * Write to specified register
- * @param reg register address
- * @parma data data to be written
- */
- void registerWrite( int reg, char data );
-
- /**
- * read gyroscope vector
- */
- std::vector<short> read( void );
+ Plane g; // gyro angular velocity readings
+ I2C i2c;
+ L3G4200D(PinName, PinName);
- /**
- * Read angular velogity (in degrees per second)
- */
- std::vector<float> angularVelocity( void );
-
- /**
- * Read temperature (in celsius)
- */
- int temperature( void );
+ void writeReg(byte reg, byte value);
+ char readReg(byte reg);
+
+ void read(void);
- // Device registers addresses
- static const int WHO_AM_I;
- static const int CTRL_REG1;
- static const int CTRL_REG2;
- static const int CTRL_REG3;
- static const int CTRL_REG4;
- static const int CTRL_REG5;
- static const int REFERENCE;
- static const int OUT_TEMP;
- static const int STATUS_REG;
- static const int OUT_X_L;
- static const int OUT_X_H;
- static const int OUT_Y_L;
- static const int OUT_Y_H;
- static const int OUT_Z_L;
- static const int OUT_Z_H;
- static const int FIFO_CTRL_REG;
- static const int FIFO_SRC_REG;
- static const int INT1_CFG;
- static const int INT1_SRC;
- static const int INT1_THS_XH;
- static const int INT1_THS_XL;
- static const int INT1_THS_YH;
- static const int INT1_THS_YL;
- static const int INT1_THS_ZH;
- static const int INT1_THS_ZL;
- static const int INT1_DURATION;
-
-private:
-
- int _status;
- I2C _i2c;
- char _bytes[7];
-
- static const int GYR_ADDRESS;
-
+ // vector functions
+ static void vector_cross(const Plane *a, const Plane *b, Plane *out);
+ static float vector_dot(const Plane *a,const Plane *b);
+ static void vector_normalize(Plane *a);
};
-#endif // L3G4200D_h
\ No newline at end of file
+#endif
diff -r dc35364e2291 -r 3272ece36ce1 LSM303.cpp
--- a/LSM303.cpp Thu Apr 12 13:47:23 2012 +0000
+++ b/LSM303.cpp Mon Apr 23 14:31:08 2012 +0000
@@ -1,209 +1,228 @@
-/* mbed LSM303 Library version 0beta1
- * Copyright (c) 2012 bengo
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
#include <LSM303.h>
-#include <cmath>
+#include "LSM303.h"
+#include <math.h>
+
+LSM303::LSM303( PinName sda = p9, PinName scl = p10 ) : i2c( sda, scl ) {
+ // 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 = LSM303DLM_DEVICE;
+ acc_address = ACC_ADDRESS_SA0_A_LOW;
+}
+
+// Public Methods //////////////////////////////////////////////////////////////
+
+void LSM303::init(byte device, byte sa0_a) {
+ Serial pc(USBTX, USBRX);
+ _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;
-// LSM303 I2C addresses
-const int LSM303::ACC_ADDRESS = 0x30;
-const int LSM303::MAG_ADDRESS = 0x3c;
-// LSM303 register addresses
-const int LSM303::ACC_CTRL_REG1 = 0x20;
-const int LSM303::ACC_CTRL_REG2 = 0x21;
-const int LSM303::ACC_CTRL_REC3 = 0x22;
-const int LSM303::ACC_CTRL_REG4 = 0x23;
-const int LSM303::ACC_CTRL_REG5 = 0x24;
-const int LSM303::ACC_HP_FILTER_RESET = 0x25;
-const int LSM303::ACC_REFERENCE = 0x26;
-const int LSM303::ACC_STATUS_REG = 0x27;
-const int LSM303::ACC_OUT_X_L = 0x28;
-const int LSM303::ACC_OUT_X_H = 0x29;
-const int LSM303::ACC_OUT_Y_L = 0x2a;
-const int LSM303::ACC_OUT_Y_H = 0x2b;
-const int LSM303::ACC_OUT_Z_L = 0x2c;
-const int LSM303::ACC_OUT_Z_H = 0x2d;
-const int LSM303::ACC_INT1_CFG = 0x30;
-const int LSM303::ACC_INT1_SOURCE = 0x31;
-const int LSM303::ACC_INT1_THS = 0x32;
-const int LSM303::ACC_INT1_DURATION = 0x33;
-const int LSM303::ACC_INT2_CFG = 0x34;
-const int LSM303::ACC_INT2_SOURCE = 0x35;
-const int LSM303::ACC_INT2_THS = 0x36;
-const int LSM303::ACC_INT2_DURATION = 0x37;
-const int LSM303::MAG_CRA_REG = 0x00;
-const int LSM303::MAG_CRB_REG = 0x01;
-const int LSM303::MAG_MR_REG = 0x02;
-const int LSM303::MAG_OUT_X_H = 0x03;
-const int LSM303::MAG_OUT_X_L = 0x04;
-const int LSM303::MAG_OUT_Y_H = 0x07;
-const int LSM303::MAG_OUT_Y_L = 0x08;
-const int LSM303::MAG_OUT_Z_H = 0x05;
-const int LSM303::MAG_OUT_Z_L = 0x6;
-const int LSM303::MAG_SR_REG = 0x9;
-const int LSM303::MAG_IRA_REG = 0x0a;
-const int LSM303::MAG_IRB_REG = 0x0b;
-const int LSM303::MAG_IRC_REG = 0x0c;
-const int LSM303::MAG_WHO_AM_I = 0x0f;
-//
+ 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) {
+ // Enable Accelerometer
+ // 0x27 = 0b00100111
+ // Normal power mode, all axes enabled
+ writeAccReg(LSM303_CTRL_REG1_A, 0x27);
-// -------------------------------------------
-LSM303::LSM303( PinName sda, PinName scl ) : _i2c( sda, scl ) {
- // Get SA0 pin status
- _bytes[0] = ACC_CTRL_REG1;
- _i2c.write( ACC_ADDRESS, _bytes, 1 );
- int sa0low = _i2c.read( ACC_ADDRESS+1, _bytes, 1 );
- _bytes[0] = ACC_CTRL_REG1;
- _i2c.write( ACC_ADDRESS+2, _bytes, 1 );
- int sa0hig = _i2c.read( ACC_ADDRESS+2+1, _bytes, 1 );
- if( sa0low == 0 && sa0hig != 0 ) {
- _SA0Pad = 0x0;
- }
- else if( sa0low != 0 && sa0hig == 0 ) {
- _SA0Pad = 0x2;
- }
- else {
- _status = 1;
- return;
- }
- // Check that you're talking with an LM303DLM device
- _bytes[0] = MAG_WHO_AM_I;
- _i2c.write( MAG_ADDRESS, _bytes, 1 );
- _status = _i2c.read( MAG_ADDRESS+1, _bytes, 1 );
- if( _bytes[0] == 0x3c ) {
- _status = 0;
- }
- else {
- _status = 1;
- return;
- }
- // Enable normal mode...
- // ... On accelerometer
- this->accRegisterWrite( ACC_CTRL_REG1, 0x27 );
- if( _status != 0 ) {
- return;
- }
- // ... And on magnetometer
- this->magRegisterWrite( MAG_MR_REG, 0x00 );
+ // Enable Magnetometer
+ // 0x00 = 0b00000000
+ // Continuous conversion mode
+ writeMagReg(LSM303_MR_REG_M, 0x00);
+}
+
+// Writes an accelerometer register
+void LSM303::writeAccReg(byte reg, byte value) {
+ char data[2] = { reg, value };
+ i2c.write( acc_address, data, 2 );
}
-LSM303::LSM303( void ) : _i2c( p9, p10 ) {}
-// -------------------------------------------
-int LSM303::accRegisterRead( int reg ) {
- _bytes[0] = reg & 0xff;
- _status = _i2c.write( ACC_ADDRESS + _SA0Pad, _bytes, 1 );
- if( _status == 0 ) {
- _status = _i2c.read( ACC_ADDRESS + _SA0Pad + 1, _bytes, 1 );
- return( _bytes[0] );
- }
- return( 0 );
+
+// Reads an accelerometer register
+int LSM303::readAccReg(byte reg) {
+ char value[1];
+ char data[1] = { reg };
+ i2c.write( acc_address, data, 1 );
+ i2c.read( acc_address, value, 1 );
+ return value[0];
+}
+
+// Writes a magnetometer register
+void LSM303::writeMagReg(byte reg, byte value) {
+ char data[2] = { reg, value };
+ i2c.write( MAG_ADDRESS, data, 2 );
}
-// -------------------------------------------
-void LSM303::accRegisterWrite( int reg, char data ) {
- _bytes[0] = reg & 0xff;
- _bytes[1] = data & 0xff;
- _status = _i2c.write( ACC_ADDRESS + _SA0Pad, _bytes, 2 );
+// Reads a magnetometer register
+int LSM303::readMagReg(int reg) {
+ char value[1];
+ char data[1];
+ // 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;
+ }
+ }
+ data[0] = reg;
+ i2c.write( MAG_ADDRESS, data, 1 );
+ i2c.read( MAG_ADDRESS, value, 1 );
+ return value[0];
+
+}
+
+// Reads the 3 accelerometer channels and stores them in vector a
+void LSM303::readAcc(void) {
+ char data[1] = { LSM303_OUT_X_L_A | (1<<7) };
+ char out[6];
+ i2c.write( acc_address, data, 1 );
+
+ i2c.read( acc_address, out, 6 );
+
+ a.x = short( ( out[1] << 8 | out[0] ) >> 4 );
+ a.y = short( ( out[3] << 8 | out[2] ) >> 4 );
+ a.z = short( ( out[5] << 8 | out[4] ) >> 4 );
+}
+
+// Reads the 3 magnetometer channels and stores them in vector m
+void LSM303::readMag(void) {
+ Serial pc(USBTX, USBRX);
+ char data[1] = { LSM303_OUT_X_H_M };
+ char out[6];
+
+ i2c.write( MAG_ADDRESS, data, 1 );
+ i2c.read( MAG_ADDRESS, out, 6 );
+
+ if (_device == LSM303DLH_DEVICE) {
+ // DLH: register address for Y comes before Z
+ m.x = short( out[0] << 8 | out[1] );
+ m.y = short( out[2] << 8 | out[3] );
+ m.z = short( out[4] << 8 | out[5] );
+ } else {
+ // DLM, DLHC: register address for Z comes before Y
+ m.x = short( out[0] << 8 | out[1] );
+ m.y = short( out[4] << 8 | out[5] );
+ m.z = short( out[2] << 8 | out[3] );
+ }
}
-// -------------------------------------------
-int LSM303::magRegisterRead( int reg ) {
- _bytes[0] = reg & 0xff;
- _status = _i2c.write( MAG_ADDRESS, _bytes, 1 );
- if( _status == 0 ) {
- _status = _i2c.read( MAG_ADDRESS + 1, _bytes, 1 );
- return( _bytes[0] );
- }
- return( 0 );
+// Reads all 6 channels of the LSM303 and stores them in the object variables
+void LSM303::read(void) {
+ readAcc();
+ readMag();
+}
+
+// Returns the number of degrees from the -Y axis that it
+// is pointing.
+int LSM303::heading(void) {
+ return heading((Plane) {
+ 0,-1,0
+ });
}
-// -------------------------------------------
-void LSM303::magRegisterWrite( int reg, char data ) {
- _bytes[0] = reg & 0xff;
- _bytes[1] = data & 0xff;
- _status = _i2c.write( MAG_ADDRESS, _bytes, 2 );
+// Returns the number of degrees from the From vector projected into
+// the horizontal plane is away from north.
+//
+// Description of heading algorithm:
+// Shift and scale the magnetic reading based on calibration data to
+// to find the North vector. Use the acceleration readings to
+// determine the Down vector. The cross product of North and Down
+// vectors is East. The vectors East and North form a basis for the
+// horizontal plane. The From vector is projected into the horizontal
+// plane and the angle between the projected vector and north is
+// returned.
+int LSM303::heading(Plane from) {
+ // shift and scale
+ m.x = (m.x - m_min.x) / (m_max.x - m_min.x) * 2 - 1.0;
+ m.y = (m.y - m_min.y) / (m_max.y - m_min.y) * 2 - 1.0;
+ m.z = (m.z - m_min.z) / (m_max.z - m_min.z) * 2 - 1.0;
+
+ Plane temp_a = a;
+ // normalize
+ vector_normalize(&temp_a);
+ //vector_normalize(&m);
+
+ // compute E and N
+ Plane E;
+ Plane N;
+ vector_cross(&m, &temp_a, &E);
+ vector_normalize(&E);
+ vector_cross(&temp_a, &E, &N);
+
+ // compute heading
+ int heading = (int)(atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / M_PI);
+ if (heading < 0) heading += 360;
+ return heading;
+}
+
+void LSM303::vector_cross( const Plane *a,const Plane *b, Plane *out ) {
+ out->x = a->y*b->z - a->z*b->y;
+ out->y = a->z*b->x - a->x*b->z;
+ out->z = a->x*b->y - a->y*b->x;
+}
+
+float LSM303::vector_dot( const Plane *a,const Plane *b ) {
+ return a->x*b->x+a->y*b->y+a->z*b->z;
+}
+
+void LSM303::vector_normalize( Plane *a ) {
+ float mag = sqrt(vector_dot(a,a));
+ a->x /= mag;
+ a->y /= mag;
+ a->z /= mag;
}
-// -------------------------------------------
-std::vector<short> LSM303::accRead( void ) {
- std::vector<short> acc( 3, 0 );
- _bytes[0] = ACC_OUT_X_L | (1<<7);
- _status = _i2c.write( ACC_ADDRESS + _SA0Pad, _bytes, 1 );
- if( _status == 0 ) {
- _status = _i2c.read( ACC_ADDRESS + _SA0Pad + 1, _bytes, 6 );
- if( _status == 0 ) {
- for( int i=0; i<3; i++ ) {
- acc[i] = ( short( _bytes[2*i] ) | short(_bytes[2*i+1]) << 8 );
- }
- }
- }
- return( acc );
-}
-
-// -------------------------------------------
-std::vector<float> LSM303::acceleration( void ) {
-
- const float cal[3][2] = { { 16291.5, -16245.4 }, { 16819.0, -16253.0 }, { 16994.8, -15525.6 } };
-
- std::vector<float> acc( 3, 0 );
- int fs = ( this->accRegisterRead( ACC_CTRL_REG4 ) >> 4 ) & 0x3;
- std::vector<short> a = this->accRead();
- if( _status == 0 ) {
- for( int i=0; i<3; i++ ) {
- acc[i] = acc[i] * ( (cal[i][0] - cal[i][1]) / 32768. ) + (cal[i][0]+cal[i][1])/2.;
- acc[i] = float( a[i] ) * pow(2.,(fs+1)) / 32768.;
- }
- }
- return( acc );
-}
-
-// -------------------------------------------
-std::vector<short> LSM303::magRead( void ) {
- std::vector<short> mag( 3, 0 );
- _bytes[0] = MAG_OUT_X_H;
- _status = _i2c.write( MAG_ADDRESS, _bytes, 1 );
- if( _status == 0 ) {
- _status = _i2c.read( MAG_ADDRESS + 1, _bytes, 6 );
- if( _status == 0 ) {
- mag[0] = (short)( _bytes[0] << 8 ) | (short)( _bytes[1] );
- mag[1] = (short)( _bytes[4] << 8 ) | (short)( _bytes[5] );
- mag[2] = (short)( _bytes[2] << 8 ) | (short)( _bytes[3] );
- }
- }
- return( mag );
-}
-
-// -------------------------------------------
-std::vector<float> LSM303::magneticField( void ) {
-
- float gainxy[] = { 1100., 855., 670., 450., 400., 330., 230. };
- float gainz[] = { 980., 760., 600., 400., 355., 295., 205. };
-
- std::vector<float> mag( 3, 0 );
- int gn = ( this->magRegisterRead( MAG_CRB_REG ) >> 5 ) & 0x7;
- std::vector<short> m = this->magRead();
- if( _status == 0 ) {
- mag[0] = float( m[0] ) / gainxy[gn-1];
- mag[1] = float( m[1] ) / gainxy[gn-1];
- mag[2] = float( m[2] ) / gainz[gn-1];
- }
- return( mag );
+byte LSM303::detectSA0_A(void) {
+ char out[1];
+ char data[1] = { LSM303_CTRL_REG1_A };
+ i2c.write( ACC_ADDRESS_SA0_A_LOW, data, 1 );
+
+ if ( 0 == i2c.read( ACC_ADDRESS_SA0_A_LOW, out, 1 ) ) {
+ return LSM303_SA0_A_LOW;
+ } else {
+ return LSM303_SA0_A_HIGH;
+ }
}
\ No newline at end of file
diff -r dc35364e2291 -r 3272ece36ce1 LSM303.h
--- a/LSM303.h Thu Apr 12 13:47:23 2012 +0000
+++ b/LSM303.h Mon Apr 23 14:31:08 2012 +0000
@@ -1,144 +1,144 @@
-/* mbed LSM303 Library version 0beta1
- * Copyright (c) 2012 bengo
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
#ifndef LSM303_h
#define LSM303_h
#include "mbed.h"
-#include <vector>
+
+// 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
+
+#define MAG_ADDRESS 0x3C
+#define ACC_ADDRESS_SA0_A_LOW 0x30
+#define ACC_ADDRESS_SA0_A_HIGH 0x32
+
+#define M_PI 3.14159265
+
+typedef unsigned char byte;
class LSM303 {
-
public:
-
- /**
- * Create an LSM303 object connected to the specified I2C pins
- * @param sda I2C SDA pin
- * @param scl I2C SCL pin
- */
- LSM303( PinName sda, PinName scl );
- LSM303( void );
-
- /**
- * Return status code of prevoius function call
- */
- inline int getStatus( void ) {
- return( _status );
- }
-
- /**
- * Read specified accelerometer register content
- * @param reg register address
- */
- int accRegisterRead( int reg );
+ typedef struct Plane {
+ float x, y, z;
+ } Plane;
+
+ I2C i2c;
+
+ Plane a; // accelerometer readings
+ Plane m; // magnetometer readings
+ Plane m_max; // maximum magnetometer values, used for calibration
+ Plane m_min; // minimum magnetometer values, used for calibration
- /**
- * Write to specified accelerometer register
- * @param reg register address
- * @parma data data to be written
- */
- void accRegisterWrite( int reg, char data );
+ LSM303(PinName, PinName);
- /**
- * Read specified magnetometer register content
- * @param reg register address
- */
- int magRegisterRead( int reg );
-
- /**
- * Write to specified magnetometer register
- * @param reg register address
- * @parma data data to be written
- */
- void magRegisterWrite( int reg, char data );
-
- /**
- * Read accelerometer vector
- */
- std::vector<short> accRead( void );
+ void init(byte device = LSM303_DEVICE_AUTO, byte sa0_a = LSM303_SA0_A_AUTO);
- /**
- * Read acceleration
- */
- std::vector<float> acceleration( void );
+ void enableDefault(void);
- /**
- * Read magnetometer vector
- */
- std::vector<short> magRead( void );
-
- /**
- * Read magnetic field vector
- */
- std::vector<float> magneticField( void );
+ void writeAccReg(byte reg, byte value);
+ int readAccReg(byte reg);
+ void writeMagReg(byte reg, byte value);
+ int readMagReg(int reg);
- // Device registers addresses
- static const int ACC_CTRL_REG1;
- static const int ACC_CTRL_REG2;
- static const int ACC_CTRL_REC3;
- static const int ACC_CTRL_REG4;
- static const int ACC_CTRL_REG5;
- static const int ACC_HP_FILTER_RESET;
- static const int ACC_REFERENCE;
- static const int ACC_STATUS_REG;
- static const int ACC_OUT_X_L;
- static const int ACC_OUT_X_H;
- static const int ACC_OUT_Y_L;
- static const int ACC_OUT_Y_H;
- static const int ACC_OUT_Z_L;
- static const int ACC_OUT_Z_H;
- static const int ACC_INT1_CFG;
- static const int ACC_INT1_SOURCE;
- static const int ACC_INT1_THS;
- static const int ACC_INT1_DURATION;
- static const int ACC_INT2_CFG;
- static const int ACC_INT2_SOURCE;
- static const int ACC_INT2_THS;
- static const int ACC_INT2_DURATION;
- static const int MAG_CRA_REG;
- static const int MAG_CRB_REG;
- static const int MAG_MR_REG;
- static const int MAG_OUT_X_H;
- static const int MAG_OUT_X_L;
- static const int MAG_OUT_Y_H;
- static const int MAG_OUT_Y_L;
- static const int MAG_OUT_Z_H;
- static const int MAG_OUT_Z_L;
- static const int MAG_SR_REG;
- static const int MAG_IRA_REG;
- static const int MAG_IRB_REG;
- static const int MAG_IRC_REG;
- static const int MAG_WHO_AM_I;
+ void readAcc(void);
+ void readMag(void);
+ void read(void);
+
+ int heading(void);
+ int heading(Plane from);
+
+ // Plane functions
+ static void vector_cross(const Plane *a, const Plane *b, Plane *out);
+ static float vector_dot(const Plane *a,const Plane *b);
+ static void vector_normalize(Plane *a);
private:
+ byte _device; // chip type (DLH, DLM, or DLHC)
+ byte acc_address;
- int _status;
- I2C _i2c;
- int _SA0Pad;
- char _bytes[7];
-
- static const int ACC_ADDRESS;
- static const int MAG_ADDRESS;
-
+ byte detectSA0_A(void);
};
-#endif // LSM303_h
\ No newline at end of file
+#endif
\ No newline at end of file
diff -r dc35364e2291 -r 3272ece36ce1 Matrix.cpp
--- a/Matrix.cpp Thu Apr 12 13:47:23 2012 +0000
+++ b/Matrix.cpp Mon Apr 23 14:31:08 2012 +0000
@@ -1,66 +1,47 @@
-/* mbed L3G4200D Library version 0.1
- * Copyright (c) 2012 Prediluted
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-//Computes the dot product of two vectors
-float Vector_Dot_Product(float vector1[3],float vector2[3]) {
- float op=0;
-
- for (int c=0; c<3; c++) {
- op+=vector1[c]*vector2[c];
- }
-
- return op;
-}
-
-//Computes the cross product of two vectors
-void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]) {
- vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
- vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
- vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
-}
-
-//Multiply the vector by a scalar.
-void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2) {
- for (int c=0; c<3; c++) {
- vectorOut[c]=vectorIn[c]*scale2;
- }
-}
-
-void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]) {
- for (int c=0; c<3; c++) {
- vectorOut[c]=vectorIn1[c]+vectorIn2[c];
- }
-}
-
-//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
-void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]) {
- float op[3];
- for (int x=0; x<3; x++) {
- for (int y=0; y<3; y++) {
- for (int w=0; w<3; w++) {
- op[w]=a[x][w]*b[w][y];
- }
- mat[x][y]=0;
- mat[x][y]=op[0]+op[1]+op[2];
- }
- }
+
+//Computes the dot product of two vectors
+float vector_dot_product(float vector1[3],float vector2[3]) {
+ float op=0;
+
+ for (int c=0; c<3; c++) {
+ op+=vector1[c]*vector2[c];
+ }
+
+ return op;
+}
+
+//Computes the cross product of two vectors
+void vector_cross_product(float vectorOut[3], float v1[3],float v2[3]) {
+ vectorOut[0]= (v1[1]*v2[2]) - (v1[2]*v2[1]);
+ vectorOut[1]= (v1[2]*v2[0]) - (v1[0]*v2[2]);
+ vectorOut[2]= (v1[0]*v2[1]) - (v1[1]*v2[0]);
+}
+
+//Multiply the vector by a scalar.
+void vector_scale(float vectorOut[3],float vectorIn[3], float scale2) {
+ for (int c=0; c<3; c++) {
+ vectorOut[c]=vectorIn[c]*scale2;
+ }
+}
+
+void vector_add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]) {
+ for (int c=0; c<3; c++) {
+ vectorOut[c]=vectorIn1[c]+vectorIn2[c];
+ }
+}
+
+//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
+void matrix_multiply(float a[3][3], float b[3][3],float mat[3][3]) {
+ float op[3];
+ for (int x=0; x<3; x++) {
+ for (int y=0; y<3; y++) {
+ for (int w=0; w<3; w++) {
+ op[w]=a[x][w]*b[w][y];
+ }
+ mat[x][y]=0;
+ mat[x][y]=op[0]+op[1]+op[2];
+
+ //float test=mat[x][y];
+ }
+ }
}
\ No newline at end of file
diff -r dc35364e2291 -r 3272ece36ce1 Matrix.h --- a/Matrix.h Thu Apr 12 13:47:23 2012 +0000 +++ b/Matrix.h Mon Apr 23 14:31:08 2012 +0000 @@ -1,5 +1,5 @@ /* mbed L3G4200D Library version 0.1 - * Copyright (c) 2012 Prediluted + * Copyright (c) 2012 2012 Ported to MBED and modified by Prediluted (Kris Reynolds) * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal @@ -20,8 +20,8 @@ * THE SOFTWARE. */ -float Vector_Dot_Product(float vector1[3],float vector2[3]); -void Vector_Cross_Product(float vectorOut[3], float v1[3],float v2[3]); -void Vector_Scale(float vectorOut[3],float vectorIn[3], float scale2); -void Vector_Add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]); -void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]); \ No newline at end of file +float vector_dot_product(float vector1[3],float vector2[3]); +void vector_cross_product(float vectorOut[3], float v1[3],float v2[3]); +void vector_scale(float vectorOut[3],float vectorIn[3], float scale2); +void vector_add(float vectorOut[3],float vectorIn1[3], float vectorIn2[3]); +void matrix_multiply(float a[3][3], float b[3][3],float mat[3][3]); \ No newline at end of file
diff -r dc35364e2291 -r 3272ece36ce1 minimu9.cpp
--- a/minimu9.cpp Thu Apr 12 13:47:23 2012 +0000
+++ b/minimu9.cpp Mon Apr 23 14:31:08 2012 +0000
@@ -1,77 +1,284 @@
-/* mbed L3G4200D Library version 0.1
- * Copyright (c) 2012 Prediluted
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-/*
- * Communication for the minimu9, ported from the original Polulu minIMU-9 Arduino code
- */
-#include <stdlib.h>
-#include <algorithm>
-#include <iostream>
-#include <vector>
#include <math.h>
+#include "mbed.h"
#include "minimu9.h"
+#include "LSM303.h"
#include "L3G4200D.h"
-#include "LSM303.h"
#include "Matrix.h"
+minimu9::minimu9( void ) {
+ Serial pc(USBTX, USBRX);
+ t.start();
+ initialize_values();
+ gyro = new L3G4200D( p9, p10 );
+ compass = new LSM303( p9, p10 );
+
+ // Initiate the accel
+ compass->writeAccReg(LSM303_CTRL_REG1_A, 0x27); // normal power mode, all axes enabled, 50 Hz
+ compass->writeAccReg(LSM303_CTRL_REG4_A, 0x30); // 8 g full scale
+
+ // Initiate the compass
+ compass->init();
+ compass->writeMagReg(LSM303_MR_REG_M, 0x00); // continuous conversion mode
+
+ // Initiate the gyro
+ gyro->writeReg(L3G4200D_CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
+ gyro->writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale
+
+ wait( 0.02 );
+
+ for (int i=0; i<32; i++) { // We take some readings...
+ read_gyro();
+ read_accel();
+ for (int y=0; y<6; y++) // Cumulate values
+ AN_OFFSET[y] += AN[y];
+ wait( 0.02 );
+ }
+
+ for (int y=0; y<6; y++)
+ AN_OFFSET[y] = AN_OFFSET[y]/32;
+
+ AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];
+
+ //Serial.println("Offset:");
+ for ( int y=0; y<6; y++ ) {
+ pc.printf( "%d\n", AN_OFFSET[y] );
+ }
+ wait( 2 );
+
+ timer=t.read_ms();
+ wait(0.02);
+ counter=0;
+}
+
+bool minimu9::update() { //Main Loop
+ if ((t.read_ms()-timer)>=20) { // Main loop runs at 50Hz
+ counter++;
+ timer_old = timer;
+ timer=t.read_ms();
+ if (timer>timer_old)
+ G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
+ else
+ G_Dt = 0;
+
+ // *** DCM algorithm
+ // Data adquisition
+ read_gyro(); // This read gyro data
+ read_accel(); // Read I2C accelerometer
+
+ if (counter > 5) { // Read compass data at 10Hz... (5 loop runs)
+ counter=0;
+ read_compass(); // Read I2C magnetometer
+ compass_heading(); // Calculate magnetic heading
+ }
+
+ // Calculations...
+ matrix_update();
+ normalize();
+ drift_correction();
+ euler_angles();
+ // ***
+
+ print_data();
+ return true;
+ }
+ return false;
+}
+
+void minimu9::read_gyro() {
+ gyro->read();
+
+ AN[0] = gyro->g.x;
+ AN[1] = gyro->g.y;
+ AN[2] = gyro->g.z;
+ gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]);
+ gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]);
+ gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]);
+}
+
+
+// Reads x,y and z accelerometer registers
+void minimu9::read_accel() {
+ compass->readAcc();
+
+ AN[3] = compass->a.x;
+ AN[4] = compass->a.y;
+ AN[5] = compass->a.z;
+ accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]);
+ accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]);
+ accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]);
+}
+
-/*
- * void setup() replaced with the constructor
- */
-minimu9::minimu9() {
- // Calibration constants defaults, will need to calibrate then use
- // set_calibration_constants() to have the proper values for your board
- M_X_MIN = -570;
- M_Y_MIN = -746;
- M_Z_MIN = -416;
- M_X_MAX = 477;
- M_Y_MAX = 324;
- M_Z_MAX = 652;
+void minimu9::read_compass() {
+ compass->readMag();
+
+ magnetom_x = SENSOR_SIGN[6] * compass->m.x;
+ magnetom_y = SENSOR_SIGN[7] * compass->m.y;
+ magnetom_z = SENSOR_SIGN[8] * compass->m.z;
+}
+
+void minimu9::compass_heading() {
+ float MAG_X;
+ float MAG_Y;
+ float cos_roll;
+ float sin_roll;
+ float cos_pitch;
+ float sin_pitch;
+
+ cos_roll = cos(roll);
+ sin_roll = sin(roll);
+ cos_pitch = cos(pitch);
+ sin_pitch = sin(pitch);
+
+ // adjust for LSM303 compass axis offsets/sensitivity differences by scaling to +/-0.5 range
+ c_magnetom_x = (float)(magnetom_x - SENSOR_SIGN[6]*M_X_MIN) / (M_X_MAX - M_X_MIN) - SENSOR_SIGN[6]*0.5;
+ c_magnetom_y = (float)(magnetom_y - SENSOR_SIGN[7]*M_Y_MIN) / (M_Y_MAX - M_Y_MIN) - SENSOR_SIGN[7]*0.5;
+ c_magnetom_z = (float)(magnetom_z - SENSOR_SIGN[8]*M_Z_MIN) / (M_Z_MAX - M_Z_MIN) - SENSOR_SIGN[8]*0.5;
+
+ // Tilt compensated Magnetic filed X:
+ MAG_X = c_magnetom_x*cos_pitch+c_magnetom_y*sin_roll*sin_pitch+c_magnetom_z*cos_roll*sin_pitch;
+ // Tilt compensated Magnetic filed Y:
+ MAG_Y = c_magnetom_y*cos_roll-c_magnetom_z*sin_roll;
+ // Magnetic Heading
+ MAG_Heading = atan2(-MAG_Y,MAG_X);
+}
- /*For debugging purposes*/
- //OUTPUTMODE=1 will print the corrected data,
- //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
- OUTPUTMODE = 1;
+void minimu9::normalize(void) {
+ float error=0;
+ float temporary[3][3];
+ float renorm=0;
+
+ error= -vector_dot_product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
+
+ vector_scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
+ vector_scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
+
+ vector_add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
+ vector_add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
+
+ vector_cross_product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
+
+ renorm= .5 *(3 - vector_dot_product(&temporary[0][0],&temporary[0][0])); //eq.21
+ vector_scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
+
+ renorm= .5 *(3 - vector_dot_product(&temporary[1][0],&temporary[1][0])); //eq.21
+ vector_scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
+
+ renorm= .5 *(3 - vector_dot_product(&temporary[2][0],&temporary[2][0])); //eq.21
+ vector_scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
+}
+
+/**************************************************/
+void minimu9::drift_correction(void) {
+ float mag_heading_x;
+ float mag_heading_y;
+ float errorCourse;
+ //Compensation the Roll, Pitch and Yaw drift.
+ static float Scaled_Omega_P[3];
+ static float Scaled_Omega_I[3];
+ float Accel_magnitude;
+ float Accel_weight;
+
+
+ //*****Roll and Pitch***************
- //#define PRINT_DCM 0 //Will print the whole direction cosine matrix
- PRINT_ANALOGS = 0; //Will print the analog raw data
- PRINT_EULER = 1; //Will print the Euler angles Roll, Pitch and Yaw
- PRINT_DCM = 0;
+ // Calculate the magnitude of the accelerometer vector
+ Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
+ Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
+ // Dynamic weighting of accelerometer info (reliability filter)
+ // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
+ Accel_weight = 1 - 2*abs(1 - Accel_magnitude),0,1; //
+ if( 1 < Accel_weight ) {
+ Accel_weight = 1;
+ }
+ else if( 0 > Accel_weight ) {
+ Accel_weight = 0;
+ }
+
+ vector_cross_product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
+ vector_scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
+
+ vector_scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
+ vector_add(Omega_I,Omega_I,Scaled_Omega_I);
+
+ //*****YAW***************
+ // We make the gyro YAW drift correction based on compass magnetic heading
- PRINT_SERIAL = 0;
- //PRINT_SERIAL = 1;
+ mag_heading_x = cos(MAG_Heading);
+ mag_heading_y = sin(MAG_Heading);
+ errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
+ vector_scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
+
+ vector_scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
+ vector_add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
+
+ vector_scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
+ vector_add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
+}
+
+
+void minimu9::matrix_update(void) {
+ Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
+ Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
+ Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw
+
+ Accel_Vector[0]=accel_x;
+ Accel_Vector[1]=accel_y;
+ Accel_Vector[2]=accel_z;
+
+ vector_add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
+ vector_add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
- t.start();
-#if PRINT_SERIAL == 1
- Serial serial(USBTX, USBRX);
+#if OUTPUTMODE==1
+ Update_Matrix[0][0]=0;
+ Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
+ Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
+ Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
+ Update_Matrix[1][1]=0;
+ Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
+ Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
+ Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
+ Update_Matrix[2][2]=0;
+#else // Uncorrected data (no drift correction)
+ Update_Matrix[0][0]=0;
+ Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
+ Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
+ Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
+ Update_Matrix[1][1]=0;
+ Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
+ Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
+ Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
+ Update_Matrix[2][2]=0;
#endif
- gyro = new L3G4200D( p9, p10 );
- compass = new LSM303( p9, p10 );
- I2C i2c(p9, p10);
+
+ matrix_multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
+
+ for (int x=0; x<3; x++) { //Matrix Addition (update)
+ for (int y=0; y<3; y++) {
+ DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
+ }
+ }
+}
- wait(1.5);
- /*
- * Give variables initial values
- */
+void minimu9::euler_angles(void) {
+ pitch = -asin(DCM_Matrix[2][0]);
+ roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+ yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
+}
+
+
+float minimu9::get_roll( void ) {
+ return ToDeg( roll );
+}
+float minimu9::get_pitch( void ) {
+ return ToDeg( pitch );
+}
+float minimu9::get_yaw( void ) {
+ return ToDeg( yaw );
+}
+
+
+void minimu9::initialize_values( void ) {
G_Dt=0.02;
timer=0;
timer24=0;
@@ -79,7 +286,8 @@
gyro_sat=0;
memcpy(SENSOR_SIGN, (int [9]) {
- 1,1,1,-1,-1,-1,1,1,1
+ 1,-1,-1,-1,1,1,1,-1,-1
+ //1,1,1,-1,-1,-1,1,1,1
}, 9*sizeof(int));
memcpy(AN_OFFSET, (int [6]) {
0,0,0,0,0,0
@@ -123,381 +331,21 @@
0,0,0
}, { 0,0,0 }, { 0,0,0 }
}, 9*sizeof(float) );
-
- /*
- * Initialize the accel, compass, and gyro
- */
- // Accel
- compass->accRegisterWrite(LSM303::ACC_CTRL_REG1, 0x24); // normal power mode, all axes enabled, 50 Hz
- compass->accRegisterWrite(LSM303::ACC_CTRL_REG4, 0x30 ); // 8g full scale
- // Compass
- compass->magRegisterWrite(LSM303::MAG_MR_REG, 0x00); // continuous conversion mode
- // Gyro
- gyro->registerWrite(L3G4200D::CTRL_REG1, 0x0F); // normal power mode, all axes enabled, 100 Hz
- gyro->registerWrite(L3G4200D::CTRL_REG4, 0x20); // 2000 dps full scale
-
-#if PRINT_SERIAL == 1
- serial.printf( "initiatied\n" );
-}
-#endif
-
-for (int i=0; i<32; i++) { // We take some readings...
- Read_Gyro();
- Read_Accel();
- for (int y=0; y<6; y++) // Cumulate values
- AN_OFFSET[y] += AN[y];
- wait(0.2);
-}
-
-for (int y=0; y<6; y++)
- AN_OFFSET[y] = AN_OFFSET[y]/32;
-
-AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];
-
-#if PRINT_SERIAL == 1
-serial.printf("Offset:\n");
-for (int y=0; y<6; y++) {
- serial.printf( "%d", AN_OFFSET[y] );
-}
-serial.printf( "\n" );
-#endif
-wait(2);
-timer=t.read_ms();
-wait(0.2);
-counter=0;
-
-#if PRINT_SERIAL == 1
-while ( 1 ) {
- get_data();
-}
-#endif
-}
-
-void minimu9::get_data() { //Main Loop
- if ((t.read_ms()-timer)>=20) { // Main loop runs at 50Hz
- counter++;
- timer_old = timer;
- timer=t.read_ms();
- if (timer>timer_old)
- G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
- else
- G_Dt = 0;
-
- // *** DCM algorithm
- // Data adquisition
- Read_Gyro(); // This read gyro data
- Read_Accel(); // Read I2C accelerometer
-
- if (counter > 5) { // Read compass data at 10Hz... (5 loop runs)
- counter=0;
- Read_Compass(); // Read I2C magnetometer
- Compass_Heading(); // Calculate magnetic heading
- }
-
- // Calculations...
- Matrix_update();
- Normalize();
- Drift_correction();
- Euler_angles();
- // ***
-
- if ( 1 == PRINT_SERIAL ) {
- print_data();
- }
- }
-
-}
-
-long convert_to_dec(float x) {
- return x*10000000;
-}
-
-void minimu9::Read_Gyro() {
- gyro->read();
- std::vector<short> g = gyro->read();
- AN[0] = g[0];
- AN[1] = g[1];
- AN[2] = g[2];
-
- gyro_x = SENSOR_SIGN[0] * (AN[0] - AN_OFFSET[0]);
- gyro_y = SENSOR_SIGN[1] * (AN[1] - AN_OFFSET[1]);
- gyro_z = SENSOR_SIGN[2] * (AN[2] - AN_OFFSET[2]);
-}
-
-
-// Reads x,y and z accelerometer registers
-void minimu9::Read_Accel() {
- std::vector<short> a = compass->accRead();
-
- AN[3] = a[0];
- AN[4] = a[1];
- AN[5] = a[2];
-
- accel_x = SENSOR_SIGN[3] * (AN[3] - AN_OFFSET[3]);
- accel_y = SENSOR_SIGN[4] * (AN[4] - AN_OFFSET[4]);
- accel_z = SENSOR_SIGN[5] * (AN[5] - AN_OFFSET[5]);
-}
-
-void minimu9::Compass_Heading() {
- float MAG_X;
- float MAG_Y;
- float cos_roll;
- float sin_roll;
- float cos_pitch;
- float sin_pitch;
-
- cos_roll = cos(roll);
- sin_roll = sin(roll);
- cos_pitch = cos(pitch);
- sin_pitch = sin(pitch);
-
- // adjust for LSM303 compass axis offsets/sensitivity differences by scaling to +/-0.5 range
- c_magnetom_x = (float)(magnetom_x - SENSOR_SIGN[6]*M_X_MIN) / (M_X_MAX - M_X_MIN) - SENSOR_SIGN[6]*0.0;
- c_magnetom_y = (float)(magnetom_y - SENSOR_SIGN[7]*M_Y_MIN) / (M_Y_MAX - M_Y_MIN) - SENSOR_SIGN[7]*-0.5;
- c_magnetom_z = (float)(magnetom_z - SENSOR_SIGN[8]*M_Z_MIN) / (M_Z_MAX - M_Z_MIN) - SENSOR_SIGN[8]*-0.5;
-
- // Tilt compensated Magnetic filed X:
- MAG_X = c_magnetom_x*cos_pitch+c_magnetom_y*sin_roll*sin_pitch+c_magnetom_z*cos_roll*sin_pitch;
- // Tilt compensated Magnetic filed Y:
- MAG_Y = c_magnetom_y*cos_roll-c_magnetom_z*sin_roll;
- // Magnetic Heading
- MAG_Heading = atan2(-MAG_Y,MAG_X);
-}
-
-void minimu9::Read_Compass() {
- std::vector<short> m = compass->magRead();
-
- magnetom_x = SENSOR_SIGN[6] * m[0];
- magnetom_y = SENSOR_SIGN[7] * m[1];
- magnetom_z = SENSOR_SIGN[8] * m[2];
-}
-
-void minimu9::Normalize(void) {
- float error=0;
- float temporary[3][3];
- float renorm=0;
-
- error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
-
- Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
- Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
-
- Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
- Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
-
- Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
-
- renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
- Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
-
- renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
- Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
-
- renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
- Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
-}
-
-/**************************************************/
-void minimu9::Drift_correction(void) {
- float mag_heading_x;
- float mag_heading_y;
- float errorCourse;
- //Compensation the Roll, Pitch and Yaw drift.
- static float Scaled_Omega_P[3];
- static float Scaled_Omega_I[3];
- float Accel_magnitude;
- float Accel_weight;
-
-
- //*****Roll and Pitch***************
-
- // Calculate the magnitude of the accelerometer vector
- Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
- Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
- // Dynamic weighting of accelerometer info (reliability filter)
- // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
- Accel_weight = 1 - 2 * abs( 1 - Accel_magnitude );
- if ( Accel_weight < 0 ) {
- Accel_weight = 0;
- } else if ( Accel_weight > 1 ) {
- Accel_weight = 1;
- }
-
- Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
- Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
-
- Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
- Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
-
- //*****YAW***************
- // We make the gyro YAW drift correction based on compass magnetic heading
-
- mag_heading_x = cos(MAG_Heading);
- mag_heading_y = sin(MAG_Heading);
- errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error
- Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
-
- Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01 proportional of YAW.
- Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
-
- Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001 Integrator
- Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
-}
-
-void minimu9::Matrix_update(void) {
- Gyro_Vector[0]=Gyro_Scaled_X(gyro_x); //gyro x roll
- Gyro_Vector[1]=Gyro_Scaled_Y(gyro_y); //gyro y pitch
- Gyro_Vector[2]=Gyro_Scaled_Z(gyro_z); //gyro Z yaw
-
- Accel_Vector[0]=accel_x;
- Accel_Vector[1]=accel_y;
- Accel_Vector[2]=accel_z;
-
- Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
- Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
-
-#if OUTPUTMODE==1
- Update_Matrix[0][0]=0;
- Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
- Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
- Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
- Update_Matrix[1][1]=0;
- Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x
- Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y
- Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x
- Update_Matrix[2][2]=0;
-#else // Uncorrected data (no drift correction)
- Update_Matrix[0][0]=0;
- Update_Matrix[0][1]=-G_Dt*Gyro_Vector[2];//-z
- Update_Matrix[0][2]=G_Dt*Gyro_Vector[1];//y
- Update_Matrix[1][0]=G_Dt*Gyro_Vector[2];//z
- Update_Matrix[1][1]=0;
- Update_Matrix[1][2]=-G_Dt*Gyro_Vector[0];
- Update_Matrix[2][0]=-G_Dt*Gyro_Vector[1];
- Update_Matrix[2][1]=G_Dt*Gyro_Vector[0];
- Update_Matrix[2][2]=0;
-#endif
-
- Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
-
- for (int x=0; x<3; x++) { //Matrix Addition (update)
- for (int y=0; y<3; y++) {
- DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
- }
- }
-}
-
-void minimu9::Euler_angles(void) {
- pitch = -asin(DCM_Matrix[2][0]);
- roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
- yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
}
void minimu9::print_data(void) {
- Serial serial(USBTX, USBRX);
-
- serial.printf("!");
+#if DEBUG_MODE == 1
+ Serial pc(USBTX, USBRX);
+ pc.printf("!");
#if PRINT_EULER == 1
- serial.printf("ANG:");
- serial.printf("%f, ", ToDeg(roll));
- serial.printf("%f, ", ToDeg(pitch));
- serial.printf("%f, ", ToDeg(yaw));
+ pc.printf("ANG:%.2f,%.2f,%.2f", ToDeg(roll), ToDeg(pitch), ToDeg(yaw));
#endif
#if PRINT_ANALOGS==1
- serial.printf(",AN:");
- serial.printf("%d, ", AN[0]);
- serial.printf("%d, ", AN[1]);
- serial.printf("%d, ", AN[2]);
- serial.printf("%d, ", AN[3]);
- serial.printf("%d, ", AN[4]);
- serial.printf("%d, ", AN[5]);
- serial.printf("%f, ", c_magnetom_x);
- serial.printf("%f, ", c_magnetom_y);
- serial.printf("%f, ", c_magnetom_z);
+ pc.printf(",AN:%d,%d,%d,%d,%d,%d", AN[0], AN[1], AN[2], AN[3], AN[4], AN[5] );
+ pc.printf(",%.2f,%.2f,%.2f", c_magnetom_x, c_magnetom_y, c_magnetom_z );
+#endif
+ pc.printf("\n");
#endif
-#if PRINT_DCM == 1
- serial.printf (",DCM:");
- serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][0]));
- serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][1]));
- serial.printf("%d, ", convert_to_dec(DCM_Matrix[0][2]));
- serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][0]));
- serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][1]));
- serial.printf("%d, ", convert_to_dec(DCM_Matrix[1][2]));
- serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][0]));
- serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][1]));
- serial.printf("%d, ", convert_to_dec(DCM_Matrix[2][2]));
-#endif
- serial.printf("\n");
}
-
-float minimu9::get_pitch( void ) {
- return ToDeg( pitch );
-}
-float minimu9::get_roll( void ) {
- return ToDeg( roll );
-}
-float minimu9::get_yaw( void ) {
- return ToDeg( yaw );
-}
-
-int min ( int a, int b ) {
- if ( b < a )
- return b;
- return a;
-}
-int max ( int a, int b ) {
- if ( b > a )
- return b;
- return a;
-}
-
-void minimu9::Calibrate_compass( void ) {
- Serial serial(USBTX, USBRX);
- int running_min[3] = {2047, 2047, 2047}, running_max[3] = {-2048, -2048, -2048};
- while ( 1 ) {
- std::vector<short> m = compass->magRead();
-
- for ( int i = 0; i < 3; i++ ) {
- running_min[i] = min(running_min[i], m[i]);
- running_max[i] = max(running_max[i], m[i]);
- }
-
- serial.printf("M min ");
- serial.printf("X: %d", (int)running_min[0] );
- serial.printf(" Y: %d", (int)running_min[1]);
- serial.printf(" Z: %d", (int)running_min[2]);
-
- serial.printf(" M max ");
- serial.printf("X: %d", (int)running_max[0] );
- serial.printf(" Y: %d", (int)running_max[1]);
- serial.printf(" Z: %d",(int)running_max[2]);
- serial.printf("\n");
-
- }
-}
-
-void minimu9::set_calibration_values( int x_min, int y_min, int z_min, int x_max, int y_max, int z_max ) {
- M_X_MIN = x_min;
- M_Y_MIN = y_min;
- M_Z_MIN = z_min;
- M_X_MAX = x_max;
- M_Y_MAX = y_max;
- M_Z_MAX = z_max;
-}
-
-void minimu9::set_print_settings( int mode, int analogs, int euler, int dcm, int serial ) {
- /*For debugging purposes*/
- //OUTPUTMODE=1 will print the corrected data,
- //OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
- OUTPUTMODE = mode;
-
- //#define PRINT_DCM 0 //Will print the whole direction cosine matrix
- PRINT_ANALOGS = analogs; //Will print the analog raw data
- PRINT_EULER = euler; //Will print the Euler angles Roll, Pitch and Yaw
- PRINT_DCM = dcm;
-
- PRINT_SERIAL = serial;
- //PRINT_SERIAL = 1;
-}
diff -r dc35364e2291 -r 3272ece36ce1 minimu9.h
--- a/minimu9.h Thu Apr 12 13:47:23 2012 +0000
+++ b/minimu9.h Mon Apr 23 14:31:08 2012 +0000
@@ -1,30 +1,39 @@
-/* mbed L3G4200D Library version 0.1
- * Copyright (c) 2012 Prediluted
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#pragma once
+/*
+
+MinIMU-9-Arduino-AHRS
+Pololu MinIMU-9 + Arduino AHRS (Attitude and Heading Reference System)
+
+Copyright (c) 2011 Pololu Corporation.
+http://www.pololu.com/
+
+MinIMU-9-Arduino-AHRS is based on sf9domahrs by Doug Weibel and Jose Julio:
+http://code.google.com/p/sf9domahrs/
+
+sf9domahrs is based on ArduIMU v1.5 by Jordi Munoz and William Premerlani, Jose
+Julio and Doug Weibel:
+http://code.google.com/p/ardu-imu/
+MinIMU-9-Arduino-AHRS 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 3 of the License, or (at your option)
+any later version.
+
+MinIMU-9-Arduino-AHRS 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 Lesser General Public License for
+more details.
+
+You should have received a copy of the GNU Lesser General Public License along
+with MinIMU-9-Arduino-AHRS. If not, see <http://www.gnu.org/licenses/>.
+
+*/
+#ifndef MINIMU9_h
+#define MINIMU9_h
+
+#include "mbed.h"
#include "L3G4200D.h"
#include "LSM303.h"
-#include "mbed.h"
+
// LSM303 accelerometer: 8 g sensitivity
// 3.8 mg/digit; 1 g = 256
#define GRAVITY 256 //this equivalent to 1G in the raw data coming from the accelerometer
@@ -41,55 +50,64 @@
#define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second
-
+// LSM303 magnetometer calibration constants; use the Calibrate example from
+// the Pololu LSM303 library to find the right values for your board
+#define M_X_MIN -796
+#define M_Y_MIN -457
+#define M_Z_MIN -424
+#define M_X_MAX 197
+#define M_Y_MAX 535
+#define M_Z_MAX 397
+
#define Kp_ROLLPITCH 0.02
#define Ki_ROLLPITCH 0.00002
#define Kp_YAW 1.2
#define Ki_YAW 0.00002
-
-#define STATUS_LED 13
+/*For debugging purposes*/
+//OUTPUTMODE=1 will print the corrected data,
+//OUTPUTMODE=0 will print uncorrected data of the gyros (with drift)
+#define OUTPUTMODE 1
+
+//#define PRINT_DCM 0 //Will print the whole direction cosine matrix
+#define PRINT_ANALOGS 0 //Will print the analog raw data
+#define PRINT_EULER 1 //Will print the Euler angles Roll, Pitch and Yaw
+
typedef unsigned char byte;
class minimu9 {
public:
- minimu9 ( void );
- void get_data ( void );
- void print_data ( void );
- void Gyro_Init();
- void Read_Gyro();
- void Accel_Init();
- void Read_Accel();
- void Compass_Init();
- void Compass_Heading();
- void Read_Compass();
- void Normalize( void );
- void Drift_correction( void );
- void Matrix_update( void) ;
- void Euler_angles( void );
- void Calibrate_compass( void );
- void set_calibration_values( int, int, int, int, int, int );
- void set_print_settings( int, int, int, int, int );
-
- float get_pitch( void );
- float get_roll( void );
- float get_yaw( void );
+ minimu9( void );
+ bool update( void );
+ float get_roll();
+ float get_pitch();
+ float get_yaw();
private:
+ void initialize_values( void );
+ void read_gyro( void );
+ void read_accel( void );
+ void read_compass( void );
+ void compass_heading( void );
+ void matrix_update( void );
+ void normalize( void );
+ void drift_correction( void );
+ void euler_angles( void );
+ void print_data( void );
+
L3G4200D *gyro;
- LSM303 *compass;
+ LSM303 *compass;
Timer t;
- float G_Dt; // Integration time (DCM algorithm) We will run the integration loop at 50Hz if possible
- long timer; //general purpuse timer
+ float G_Dt; // Integration time (DCM algorithm) We will run the integration loop at 50Hz if possible
+
+ long timer; //general purpuse timer
long timer_old;
- long timer24; //Second timer used to print values
- unsigned int counter;
- int AN[6]; //array that stores the gyro and accelerometer data
- int AN_OFFSET[6]; //Array that stores the Offset of the sensors
- int SENSOR_SIGN[9];
-
+ long timer24; //Second timer used to print values
+ int AN[6]; //array that stores the gyro and accelerometer data
+ int AN_OFFSET[6]; //Array that stores the Offset of the sensors
+ int SENSOR_SIGN[9]; // Orientation
int gyro_x;
int gyro_y;
int gyro_z;
@@ -119,25 +137,12 @@
float errorRollPitch[3];
float errorYaw[3];
+ unsigned int counter;
byte gyro_sat;
- // Gyros here
float DCM_Matrix[3][3];
- float Update_Matrix[3][3];
+ float Update_Matrix[3][3]; //Gyros here
float Temporary_Matrix[3][3];
+};
- // Calibration values
- int M_X_MIN;
- int M_Y_MIN;
- int M_Z_MIN;
- int M_X_MAX;
- int M_Y_MAX;
- int M_Z_MAX;
-
- int OUTPUTMODE;
- int PRINT_ANALOGS;
- int PRINT_EULER;
- int PRINT_DCM;
- int PRINT_SERIAL;
-
-};
\ No newline at end of file
+#endif
\ No newline at end of file