Aloïs Wolff
mbed implementation of the FreeIMU IMU for HobbyKing's 10DOF board
Revision 0:9a1682a09c50, committed 2013-07-17
- Comitter:
- pommzorz
- Date:
- Wed Jul 17 18:50:28 2013 +0000
- Child:
- 1:85fcfcb7b137
- Commit message:
- mbed implementation of the FreeIMU imu for the HobbyKing 10DOF board
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345_I2C.cpp Wed Jul 17 18:50:28 2013 +0000
@@ -0,0 +1,447 @@
+/**
+ * @author Peter Swanson
+ * A personal note from me: Jesus Christ has changed my life so much it blows my mind. I say this because
+ * today, religion is thought of as something that you do or believe and has about as
+ * little impact on a person as their political stance. But for me, God gives me daily
+ * strength and has filled my life with the satisfaction that I could never find in any
+ * of the other things that I once looked for it in.
+ * If your interested, heres verse that changed my life:
+ * Rom 8:1-3: "Therefore, there is now no condemnation for those who are in Christ Jesus,
+ * because through Christ Jesus, the law of the Spirit who gives life has set
+ * me free from the law of sin (which brings...) and death. For what the law
+ * was powerless to do in that it was weakened by the flesh, God did by sending
+ * His own Son in the likeness of sinful flesh to be a sin offering. And so He
+ * condemned sin in the flesh in order that the righteous requirements of the
+ * (God's) law might be fully met in us, who live not according to the flesh
+ * but according to the Spirit."
+ *
+ * A special thanks to Ewout van Bekkum for all his patient help in developing this library!
+ *
+ * @section LICENSE
+ *
+ * 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.
+ *
+ * @section DESCRIPTION
+ *
+ * ADXL345, triple axis, I2C interface, accelerometer.
+ *
+ * Datasheet:
+ *
+ * http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf
+ */
+
+/**
+ * Includes
+ */
+#include "ADXL345_I2C.h"
+
+//#include "mbed.h"
+
+ADXL345_I2C::ADXL345_I2C(PinName sda, PinName scl) : i2c_(sda, scl) {
+
+ //400kHz, allowing us to use the fastest data rates.
+ i2c_.frequency(400000); //400000
+// initialize the BW data rate
+ char tx[2];
+ tx[0] = ADXL345_BW_RATE_REG;
+ tx[1] = ADXL345_1600HZ; //value greater than or equal to 0x0A is written into the rate bits (Bit D3 through Bit D0) in the BW_RATE register
+ i2c_.write( ADXL345_I2C_WRITE , tx, 2);
+
+//Data format (for +-16g) - This is done by setting Bit D3 of the DATA_FORMAT register (Address 0x31) and writing a value of 0x03 to the range bits (Bit D1 and Bit D0) of the DATA_FORMAT register (Address 0x31).
+
+ char rx[2];
+ rx[0] = ADXL345_DATA_FORMAT_REG;
+ rx[1] = 0x0B;
+ // full res and +_16g
+ i2c_.write( ADXL345_I2C_WRITE , rx, 2);
+
+ // Set Offset - programmed into the OFSX, OFSY, and OFXZ registers, respectively, as 0xFD, 0x03 and 0xFE.
+ char x[2];
+ x[0] = ADXL345_OFSX_REG ;
+ x[1] = 0xFD;
+ i2c_.write( ADXL345_I2C_WRITE , x, 2);
+ char y[2];
+ y[0] = ADXL345_OFSY_REG ;
+ y[1] = 0x03;
+ i2c_.write( ADXL345_I2C_WRITE , y, 2);
+ char z[2];
+ z[0] = ADXL345_OFSZ_REG ;
+ z[1] = 0xFE;
+ i2c_.write( ADXL345_I2C_WRITE , z, 2);
+}
+
+
+char ADXL345_I2C::SingleByteRead(char address){
+ char tx = address;
+ char output;
+ i2c_.write( ADXL345_I2C_WRITE , &tx, 1); //tell it what you want to read
+ i2c_.read( ADXL345_I2C_READ , &output, 1); //tell it where to store the data
+ return output;
+
+}
+
+
+/*
+***info on the i2c_.write***
+address 8-bit I2C slave address [ addr | 0 ]
+data Pointer to the byte-array data to send
+length Number of bytes to send
+repeated Repeated start, true - do not send stop at end
+returns 0 on success (ack), or non-0 on failure (nack)
+*/
+
+int ADXL345_I2C::SingleByteWrite(char address, char data){
+ int ack = 0;
+ char tx[2];
+ tx[0] = address;
+ tx[1] = data;
+ return ack | i2c_.write( ADXL345_I2C_WRITE , tx, 2);
+}
+
+
+
+void ADXL345_I2C::multiByteRead(char address, char* output, int size) {
+ i2c_.write( ADXL345_I2C_WRITE, &address, 1); //tell it where to read from
+ i2c_.read( ADXL345_I2C_READ , output, size); //tell it where to store the data read
+}
+
+
+int ADXL345_I2C::multiByteWrite(char address, char* ptr_data, int size) {
+ int ack;
+
+ ack = i2c_.write( ADXL345_I2C_WRITE, &address, 1); //tell it where to write to
+ return ack | i2c_.write( ADXL345_I2C_READ, ptr_data, size); //tell it what data to write
+
+}
+
+
+void ADXL345_I2C::getOutput(int16_t* readings){
+ char buffer[6];
+ multiByteRead(ADXL345_DATAX0_REG, buffer, 6);
+
+ readings[0] = (int)buffer[1] << 8 | (int)buffer[0];
+ readings[1] = (int)buffer[3] << 8 | (int)buffer[2];
+ readings[2] = (int)buffer[5] << 8 | (int)buffer[4];
+
+}
+
+/*
+
+void ADXL345_I2C::getOutput(int* readings){
+ char buffer[6];
+ multiByteRead(ADXL345_DATAX0_REG, buffer, 6);
+
+ readings[0] = (int)buffer[1] << 8 | (int)buffer[0];
+ readings[1] = (int)buffer[3] << 8 | (int)buffer[2];
+ readings[2] = (int)buffer[5] << 8 | (int)buffer[4];
+
+}
+*/
+
+void ADXL345_I2C::getOutput(int16_t* x, int16_t* y, int16_t* z) {
+ int16_t *readings;
+ getOutput(readings);
+
+ // each axis reading comes in 10 bit resolution, ie 2 bytes. Least Significat Byte first!!
+ // thus we are converting both bytes in to one int
+ *x = readings[0];
+ *y = readings[1];
+ *z = readings[2];
+}
+
+
+char ADXL345_I2C::getDeviceID() {
+ return SingleByteRead(ADXL345_DEVID_REG);
+ }
+//
+int ADXL345_I2C::setPowerMode(char mode) {
+
+ //Get the current register contents, so we don't clobber the rate value.
+ char registerContents = (mode << 4) | SingleByteRead(ADXL345_BW_RATE_REG);
+
+ return SingleByteWrite(ADXL345_BW_RATE_REG, registerContents);
+
+}
+
+char ADXL345_I2C::getPowerControl() {
+ return SingleByteRead(ADXL345_POWER_CTL_REG);
+}
+
+int ADXL345_I2C::setPowerControl(char settings) {
+ return SingleByteWrite(ADXL345_POWER_CTL_REG, settings);
+
+}
+
+
+
+char ADXL345_I2C::getDataFormatControl(void){
+
+ return SingleByteRead(ADXL345_DATA_FORMAT_REG);
+}
+
+int ADXL345_I2C::setDataFormatControl(char settings){
+
+ return SingleByteWrite(ADXL345_DATA_FORMAT_REG, settings);
+
+}
+
+int ADXL345_I2C::setDataRate(char rate) {
+
+ //Get the current register contents, so we don't clobber the power bit.
+ char registerContents = SingleByteRead(ADXL345_BW_RATE_REG);
+
+ registerContents &= 0x10;
+ registerContents |= rate;
+
+ return SingleByteWrite(ADXL345_BW_RATE_REG, registerContents);
+
+}
+
+
+char ADXL345_I2C::getOffset(char axis) {
+
+ char address = 0;
+
+ if (axis == ADXL345_X) {
+ address = ADXL345_OFSX_REG;
+ } else if (axis == ADXL345_Y) {
+ address = ADXL345_OFSY_REG;
+ } else if (axis == ADXL345_Z) {
+ address = ADXL345_OFSZ_REG;
+ }
+
+ return SingleByteRead(address);
+}
+
+int ADXL345_I2C::setOffset(char axis, char offset) {
+
+ char address = 0;
+
+ if (axis == ADXL345_X) {
+ address = ADXL345_OFSX_REG;
+ } else if (axis == ADXL345_Y) {
+ address = ADXL345_OFSY_REG;
+ } else if (axis == ADXL345_Z) {
+ address = ADXL345_OFSZ_REG;
+ }
+
+ return SingleByteWrite(address, offset);
+
+}
+
+
+char ADXL345_I2C::getFifoControl(void){
+
+ return SingleByteRead(ADXL345_FIFO_CTL);
+
+}
+
+int ADXL345_I2C::setFifoControl(char settings){
+ return SingleByteWrite(ADXL345_FIFO_STATUS, settings);
+
+}
+
+char ADXL345_I2C::getFifoStatus(void){
+
+ return SingleByteRead(ADXL345_FIFO_STATUS);
+
+}
+
+
+
+char ADXL345_I2C::getTapThreshold(void) {
+
+ return SingleByteRead(ADXL345_THRESH_TAP_REG);
+}
+
+int ADXL345_I2C::setTapThreshold(char threshold) {
+
+ return SingleByteWrite(ADXL345_THRESH_TAP_REG, threshold);
+
+}
+
+
+float ADXL345_I2C::getTapDuration(void) {
+
+ return (float)SingleByteRead(ADXL345_DUR_REG)*625;
+}
+
+int ADXL345_I2C::setTapDuration(short int duration_us) {
+
+ short int tapDuration = duration_us / 625;
+ char tapChar[2];
+ tapChar[0] = (tapDuration & 0x00FF);
+ tapChar[1] = (tapDuration >> 8) & 0x00FF;
+ return multiByteWrite(ADXL345_DUR_REG, tapChar, 2);
+
+}
+
+float ADXL345_I2C::getTapLatency(void) {
+
+ return (float)SingleByteRead(ADXL345_LATENT_REG)*1.25;
+}
+
+int ADXL345_I2C::setTapLatency(short int latency_ms) {
+
+ latency_ms = latency_ms / 1.25;
+ char latChar[2];
+ latChar[0] = (latency_ms & 0x00FF);
+ latChar[1] = (latency_ms << 8) & 0xFF00;
+ return multiByteWrite(ADXL345_LATENT_REG, latChar, 2);
+
+}
+
+float ADXL345_I2C::getWindowTime(void) {
+
+ return (float)SingleByteRead(ADXL345_WINDOW_REG)*1.25;
+}
+
+int ADXL345_I2C::setWindowTime(short int window_ms) {
+
+ window_ms = window_ms / 1.25;
+ char windowChar[2];
+ windowChar[0] = (window_ms & 0x00FF);
+ windowChar[1] = ((window_ms << 8) & 0xFF00);
+ return multiByteWrite(ADXL345_WINDOW_REG, windowChar, 2);
+
+}
+
+char ADXL345_I2C::getActivityThreshold(void) {
+
+ return SingleByteRead(ADXL345_THRESH_ACT_REG);
+}
+
+int ADXL345_I2C::setActivityThreshold(char threshold) {
+ return SingleByteWrite(ADXL345_THRESH_ACT_REG, threshold);
+
+}
+
+char ADXL345_I2C::getInactivityThreshold(void) {
+ return SingleByteRead(ADXL345_THRESH_INACT_REG);
+
+}
+
+//int FUNCTION(short int * ptr_Output)
+//short int FUNCTION ()
+
+int ADXL345_I2C::setInactivityThreshold(char threshold) {
+ return SingleByteWrite(ADXL345_THRESH_INACT_REG, threshold);
+
+}
+
+char ADXL345_I2C::getTimeInactivity(void) {
+
+ return SingleByteRead(ADXL345_TIME_INACT_REG);
+
+}
+
+int ADXL345_I2C::setTimeInactivity(char timeInactivity) {
+ return SingleByteWrite(ADXL345_TIME_INACT_REG, timeInactivity);
+
+}
+
+char ADXL345_I2C::getActivityInactivityControl(void) {
+
+ return SingleByteRead(ADXL345_ACT_INACT_CTL_REG);
+
+}
+
+int ADXL345_I2C::setActivityInactivityControl(char settings) {
+ return SingleByteWrite(ADXL345_ACT_INACT_CTL_REG, settings);
+
+}
+
+char ADXL345_I2C::getFreefallThreshold(void) {
+
+ return SingleByteRead(ADXL345_THRESH_FF_REG);
+
+}
+
+int ADXL345_I2C::setFreefallThreshold(char threshold) {
+ return SingleByteWrite(ADXL345_THRESH_FF_REG, threshold);
+
+}
+
+char ADXL345_I2C::getFreefallTime(void) {
+
+ return SingleByteRead(ADXL345_TIME_FF_REG)*5;
+
+}
+
+int ADXL345_I2C::setFreefallTime(short int freefallTime_ms) {
+ freefallTime_ms = freefallTime_ms / 5;
+ char fallChar[2];
+ fallChar[0] = (freefallTime_ms & 0x00FF);
+ fallChar[1] = (freefallTime_ms << 8) & 0xFF00;
+
+ return multiByteWrite(ADXL345_TIME_FF_REG, fallChar, 2);
+
+}
+
+char ADXL345_I2C::getTapAxisControl(void) {
+
+ return SingleByteRead(ADXL345_TAP_AXES_REG);
+
+}
+
+int ADXL345_I2C::setTapAxisControl(char settings) {
+ return SingleByteWrite(ADXL345_TAP_AXES_REG, settings);
+
+}
+
+char ADXL345_I2C::getTapSource(void) {
+
+ return SingleByteRead(ADXL345_ACT_TAP_STATUS_REG);
+
+}
+
+
+
+char ADXL345_I2C::getInterruptEnableControl(void) {
+
+ return SingleByteRead(ADXL345_INT_ENABLE_REG);
+
+}
+
+int ADXL345_I2C::setInterruptEnableControl(char settings) {
+ return SingleByteWrite(ADXL345_INT_ENABLE_REG, settings);
+
+}
+
+char ADXL345_I2C::getInterruptMappingControl(void) {
+
+ return SingleByteRead(ADXL345_INT_MAP_REG);
+
+}
+
+int ADXL345_I2C::setInterruptMappingControl(char settings) {
+ return SingleByteWrite(ADXL345_INT_MAP_REG, settings);
+
+}
+
+char ADXL345_I2C::getInterruptSource(void){
+
+ return SingleByteRead(ADXL345_INT_SOURCE_REG);
+
+}
+
+
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345_I2C.h Wed Jul 17 18:50:28 2013 +0000
@@ -0,0 +1,579 @@
+/**
+ * @author Peter Swanson
+ * A personal note from me: Jesus Christ has changed my life so much it blows my mind. I say this because
+ * today, religion is thought of as something that you do or believe and has about as
+ * little impact on a person as their political stance. But for me, God gives me daily
+ * strength and has filled my life with the satisfaction that I could never find in any
+ * of the other things that I once looked for it in.
+ * If your interested, heres verse that changed my life:
+ * Rom 8:1-3: "Therefore, there is now no condemnation for those who are in Christ Jesus,
+ * because through Christ Jesus, the law of the Spirit who gives life has set
+ * me free from the law of sin (which brings...) and death. For what the law
+ * was powerless to do in that it was weakened by the flesh, God did by sending
+ * His own Son in the likeness of sinful flesh to be a sin offering. And so He
+ * condemned sin in the flesh in order that the righteous requirements of the
+ * (God's) law might be fully met in us, who live not according to the flesh
+ * but according to the Spirit."
+ *
+ * A special thanks to Ewout van Bekkum for all his patient help in developing this library!
+ *
+ * @section LICENSE
+ *
+ * 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.
+ *
+ * @section DESCRIPTION
+ *
+ * ADXL345, triple axis, I2C interface, accelerometer.
+ *
+ * Datasheet:
+ *
+ * http://www.analog.com/static/imported-files/data_sheets/ADXL345.pdf
+ */
+
+
+
+#ifndef ADXL345_I2C_H
+#define ADXL345_I2C_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+//Registers.
+#define ADXL345_DEVID_REG 0x00
+#define ADXL345_THRESH_TAP_REG 0x1D
+#define ADXL345_OFSX_REG 0x1E
+#define ADXL345_OFSY_REG 0x1F
+#define ADXL345_OFSZ_REG 0x20
+#define ADXL345_DUR_REG 0x21
+#define ADXL345_LATENT_REG 0x22
+#define ADXL345_WINDOW_REG 0x23
+#define ADXL345_THRESH_ACT_REG 0x24
+#define ADXL345_THRESH_INACT_REG 0x25
+#define ADXL345_TIME_INACT_REG 0x26
+#define ADXL345_ACT_INACT_CTL_REG 0x27
+#define ADXL345_THRESH_FF_REG 0x28
+#define ADXL345_TIME_FF_REG 0x29
+#define ADXL345_TAP_AXES_REG 0x2A
+#define ADXL345_ACT_TAP_STATUS_REG 0x2B
+#define ADXL345_BW_RATE_REG 0x2C
+#define ADXL345_POWER_CTL_REG 0x2D
+#define ADXL345_INT_ENABLE_REG 0x2E
+#define ADXL345_INT_MAP_REG 0x2F
+#define ADXL345_INT_SOURCE_REG 0x30
+#define ADXL345_DATA_FORMAT_REG 0x31
+#define ADXL345_DATAX0_REG 0x32
+#define ADXL345_DATAX1_REG 0x33
+#define ADXL345_DATAY0_REG 0x34
+#define ADXL345_DATAY1_REG 0x35
+#define ADXL345_DATAZ0_REG 0x36
+#define ADXL345_DATAZ1_REG 0x37
+#define ADXL345_FIFO_CTL 0x38
+#define ADXL345_FIFO_STATUS 0x39
+
+//Data rate codes.
+#define ADXL345_3200HZ 0x0F
+#define ADXL345_1600HZ 0x0E
+#define ADXL345_800HZ 0x0D
+#define ADXL345_400HZ 0x0C
+#define ADXL345_200HZ 0x0B
+#define ADXL345_100HZ 0x0A
+#define ADXL345_50HZ 0x09
+#define ADXL345_25HZ 0x08
+#define ADXL345_12HZ5 0x07
+#define ADXL345_6HZ25 0x06
+
+// read or write bytes
+#define ADXL345_I2C_READ 0xA7
+#define ADXL345_I2C_WRITE 0xA6
+#define ADXL345_I2C_ADDRESS 0x53 //the ADXL345 7-bit address is 0x53 when ALT ADDRESS is low as it is on the sparkfun chip: when ALT ADDRESS is high the address is 0x1D
+
+/////////////when ALT ADDRESS pin is high:
+//#define ADXL345_I2C_READ 0x3B
+//#define ADXL345_I2C_WRITE 0x3A
+//#define ADXL345_I2C_ADDRESS 0x1D
+
+#define ADXL345_X 0x00
+#define ADXL345_Y 0x01
+#define ADXL345_Z 0x02
+
+
+
+// modes
+#define MeasurementMode 0x08
+
+
+
+
+
+
+
+class ADXL345_I2C {
+
+public:
+
+ /**
+ * Constructor.
+ *
+ * @param mosi mbed pin to use for SDA line of I2C interface.
+ * @param sck mbed pin to use for SCL line of I2C interface.
+ */
+ ADXL345_I2C(PinName sda, PinName scl);
+
+ /**
+ * Get the output of all three axes.
+ *
+ * @param Pointer to a buffer to hold the accelerometer value for the
+ * x-axis, y-axis and z-axis [in that order].
+ */
+ void getOutput(int16_t* readings);
+ void getOutput(int16_t* x, int16_t* y, int16_t* z);
+
+ /**
+ * Read the device ID register on the device.
+ *
+ * @return The device ID code [0xE5]
+ */
+ char getDeviceID(void);
+
+
+
+ /**
+ * Set the power mode.
+ *
+ * @param mode 0 -> Normal operation.
+ * 1 -> Reduced power operation.
+ */
+int setPowerMode(char mode);
+
+ /**
+ * Set the power control settings.
+ *
+ * See datasheet for details.
+ *
+ * @param The control byte to write to the POWER_CTL register.
+ */
+ int setPowerControl(char settings);
+ /**
+ * Get the power control settings.
+ *
+ * See datasheet for details.
+ *
+ * @return The contents of the POWER_CTL register.
+ */
+ char getPowerControl(void);
+
+
+ /**
+ * Get the data format settings.
+ *
+ * @return The contents of the DATA_FORMAT register.
+ */
+
+ char getDataFormatControl(void);
+
+ /**
+ * Set the data format settings.
+ *
+ * @param settings The control byte to write to the DATA_FORMAT register.
+ */
+ int setDataFormatControl(char settings);
+
+ /**
+ * Set the data rate.
+ *
+ * @param rate The rate code (see #defines or datasheet).
+ */
+ int setDataRate(char rate);
+
+
+ /**
+ * Get the current offset for a particular axis.
+ *
+ * @param axis 0x00 -> X-axis
+ * 0x01 -> Y-axis
+ * 0x02 -> Z-axis
+ * @return The current offset as an 8-bit 2's complement number with scale
+ * factor 15.6mg/LSB.
+ */
+
+ char getOffset(char axis);
+
+ /**
+ * Set the offset for a particular axis.
+ *
+ * @param axis 0x00 -> X-axis
+ * 0x01 -> Y-axis
+ * 0x02 -> Z-axis
+ * @param offset The offset as an 8-bit 2's complement number with scale
+ * factor 15.6mg/LSB.
+ */
+ int setOffset(char axis, char offset);
+
+
+
+ /**
+ * Get the FIFO control settings.
+ *
+ * @return The contents of the FIFO_CTL register.
+ */
+ char getFifoControl(void);
+
+ /**
+ * Set the FIFO control settings.
+ *
+ * @param The control byte to write to the FIFO_CTL register.
+ */
+ int setFifoControl(char settings);
+
+ /**
+ * Get FIFO status.
+ *
+ * @return The contents of the FIFO_STATUS register.
+ */
+ char getFifoStatus(void);
+
+ /**
+ * Read the tap threshold on the device.
+ *
+ * @return The tap threshold as an 8-bit number with a scale factor of
+ * 62.5mg/LSB.
+ */
+ char getTapThreshold(void);
+
+ /**
+ * Set the tap threshold.
+ *
+ * @param The tap threshold as an 8-bit number with a scale factor of
+ * 62.5mg/LSB.
+ */
+ int setTapThreshold(char threshold);
+
+ /**
+ * Get the tap duration required to trigger an event.
+ *
+ * @return The max time that an event must be above the tap threshold to
+ * qualify as a tap event, in microseconds.
+ */
+ float getTapDuration(void);
+
+ /**
+ * Set the tap duration required to trigger an event.
+ *
+ * @param duration_us The max time that an event must be above the tap
+ * threshold to qualify as a tap event, in microseconds.
+ * Time will be normalized by the scale factor which is
+ * 625us/LSB. A value of 0 disables the single/double
+ * tap functions.
+ */
+ int setTapDuration(short int duration_us);
+
+ /**
+ * Get the tap latency between the detection of a tap and the time window.
+ *
+ * @return The wait time from the detection of a tap event to the start of
+ * the time window during which a possible second tap event can be
+ * detected in milliseconds.
+ */
+ float getTapLatency(void);
+
+ /**
+ * Set the tap latency between the detection of a tap and the time window.
+ *
+ * @param latency_ms The wait time from the detection of a tap event to the
+ * start of the time window during which a possible
+ * second tap event can be detected in milliseconds.
+ * A value of 0 disables the double tap function.
+ */
+ int setTapLatency(short int latency_ms);
+
+ /**
+ * Get the time of window between tap latency and a double tap.
+ *
+ * @return The amount of time after the expiration of the latency time
+ * during which a second valid tap can begin, in milliseconds.
+ */
+ float getWindowTime(void);
+
+ /**
+ * Set the time of the window between tap latency and a double tap.
+ *
+ * @param window_ms The amount of time after the expiration of the latency
+ * time during which a second valid tap can begin,
+ * in milliseconds.
+ */
+ int setWindowTime(short int window_ms);
+
+ /**
+ * Get the threshold value for detecting activity.
+ *
+ * @return The threshold value for detecting activity as an 8-bit number.
+ * Scale factor is 62.5mg/LSB.
+ */
+ char getActivityThreshold(void);
+
+ /**
+ * Set the threshold value for detecting activity.
+ *
+ * @param threshold The threshold value for detecting activity as an 8-bit
+ * number. Scale factor is 62.5mg/LSB. A value of 0 may
+ * result in undesirable behavior if the activity
+ * interrupt is enabled.
+ */
+ int setActivityThreshold(char threshold);
+
+ /**
+ * Get the threshold value for detecting inactivity.
+ *
+ * @return The threshold value for detecting inactivity as an 8-bit number.
+ * Scale factor is 62.5mg/LSB.
+ */
+ char getInactivityThreshold(void);
+
+ /**
+ * Set the threshold value for detecting inactivity.
+ *
+ * @param threshold The threshold value for detecting inactivity as an
+ * 8-bit number. Scale factor is 62.5mg/LSB.
+ */
+ int setInactivityThreshold(char threshold);
+
+ /**
+ * Get the time required for inactivity to be declared.
+ *
+ * @return The amount of time that acceleration must be less than the
+ * inactivity threshold for inactivity to be declared, in
+ * seconds.
+ */
+ char getTimeInactivity(void);
+
+ /**
+ * Set the time required for inactivity to be declared.
+ *
+ * @param inactivity The amount of time that acceleration must be less than
+ * the inactivity threshold for inactivity to be
+ * declared, in seconds. A value of 0 results in an
+ * interrupt when the output data is less than the
+ * threshold inactivity.
+ */
+ int setTimeInactivity(char timeInactivity);
+
+ /**
+ * Get the activity/inactivity control settings.
+ *
+ * D7 D6 D5 D4
+ * +-----------+--------------+--------------+--------------+
+ * | ACT ac/dc | ACT_X enable | ACT_Y enable | ACT_Z enable |
+ * +-----------+--------------+--------------+--------------+
+ *
+ * D3 D2 D1 D0
+ * +-------------+----------------+----------------+----------------+
+ * | INACT ac/dc | INACT_X enable | INACT_Y enable | INACT_Z enable |
+ * +-------------+----------------+----------------+----------------+
+ *
+ * See datasheet for details.
+ *
+ * @return The contents of the ACT_INACT_CTL register.
+ */
+ char getActivityInactivityControl(void);
+
+ /**
+ * Set the activity/inactivity control settings.
+ *
+ * D7 D6 D5 D4
+ * +-----------+--------------+--------------+--------------+
+ * | ACT ac/dc | ACT_X enable | ACT_Y enable | ACT_Z enable |
+ * +-----------+--------------+--------------+--------------+
+ *
+ * D3 D2 D1 D0
+ * +-------------+----------------+----------------+----------------+
+ * | INACT ac/dc | INACT_X enable | INACT_Y enable | INACT_Z enable |
+ * +-------------+----------------+----------------+----------------+
+ *
+ * See datasheet for details.
+ *
+ * @param settings The control byte to write to the ACT_INACT_CTL register.
+ */
+ int setActivityInactivityControl(char settings);
+
+ /**
+ * Get the threshold for free fall detection.
+ *
+ * @return The threshold value for free-fall detection, as an 8-bit number,
+ * with scale factor 62.5mg/LSB.
+ */
+ char getFreefallThreshold(void);
+
+ /**
+ * Set the threshold for free fall detection.
+ *
+ * @return The threshold value for free-fall detection, as an 8-bit number,
+ * with scale factor 62.5mg/LSB. A value of 0 may result in
+ * undesirable behavior if the free-fall interrupt is enabled.
+ * Values between 300 mg and 600 mg (0x05 to 0x09) are recommended.
+ */
+ int setFreefallThreshold(char threshold);
+
+ /**
+ * Get the time required to generate a free fall interrupt.
+ *
+ * @return The minimum time that the value of all axes must be less than
+ * the freefall threshold to generate a free-fall interrupt, in
+ * milliseconds.
+ */
+ char getFreefallTime(void);
+
+ /**
+ * Set the time required to generate a free fall interrupt.
+ *
+ * @return The minimum time that the value of all axes must be less than
+ * the freefall threshold to generate a free-fall interrupt, in
+ * milliseconds. A value of 0 may result in undesirable behavior
+ * if the free-fall interrupt is enabled. Values between 100 ms
+ * and 350 ms (0x14 to 0x46) are recommended.
+ */
+ int setFreefallTime(short int freefallTime_ms);
+
+ /**
+ * Get the axis tap settings.
+ *
+ * D3 D2 D1 D0
+ * +----------+--------------+--------------+--------------+
+ * | Suppress | TAP_X enable | TAP_Y enable | TAP_Z enable |
+ * +----------+--------------+--------------+--------------+
+ *
+ * (D7-D4 are 0s).
+ *
+ * See datasheet for more details.
+ *
+ * @return The contents of the TAP_AXES register.
+ */
+ char getTapAxisControl(void);
+
+ /**
+ * Set the axis tap settings.
+ *
+ * D3 D2 D1 D0
+ * +----------+--------------+--------------+--------------+
+ * | Suppress | TAP_X enable | TAP_Y enable | TAP_Z enable |
+ * +----------+--------------+--------------+--------------+
+ *
+ * (D7-D4 are 0s).
+ *
+ * See datasheet for more details.
+ *
+ * @param The control byte to write to the TAP_AXES register.
+ */
+ int setTapAxisControl(char settings);
+
+ /**
+ * Get the source of a tap.
+ *
+ * @return The contents of the ACT_TAP_STATUS register.
+ */
+ char getTapSource(void);
+
+ /**
+ * Get the interrupt enable settings.
+ *
+ * @return The contents of the INT_ENABLE register.
+ */
+
+ char getInterruptEnableControl(void);
+
+ /**
+ * Set the interrupt enable settings.
+ *
+ * @param settings The control byte to write to the INT_ENABLE register.
+ */
+ int setInterruptEnableControl(char settings);
+
+ /**
+ * Get the interrupt mapping settings.
+ *
+ * @return The contents of the INT_MAP register.
+ */
+ char getInterruptMappingControl(void);
+
+ /**
+ * Set the interrupt mapping settings.
+ *
+ * @param settings The control byte to write to the INT_MAP register.
+ */
+ int setInterruptMappingControl(char settings);
+
+ /**
+ * Get the interrupt source.
+ *
+ * @return The contents of the INT_SOURCE register.
+ */
+ char getInterruptSource(void);
+
+
+private:
+
+ I2C i2c_;
+
+
+ /**
+ * Read one byte from a register on the device.
+ *
+ * @param: - the address to be read from
+ *
+ * @return: the value of the data read
+ */
+ char SingleByteRead(char address);
+
+ /**
+ * Write one byte to a register on the device.
+ *
+ * @param:
+ - address of the register to write to.
+ - the value of the data to store
+ */
+
+
+ int SingleByteWrite(char address, char data);
+
+ /**
+ * Read several consecutive bytes on the device and store them in a given location.
+ *
+ * @param startAddress: The address of the first register to read from.
+ * @param ptr_output: a pointer to the location to store the data being read
+ * @param size: The number of bytes to read.
+ */
+ void multiByteRead(char startAddress, char* ptr_output, int size);
+
+ /**
+ * Write several consecutive bytes on the device.
+ *
+ * @param startAddress: The address of the first register to write to.
+ * @param ptr_data: Pointer to a location which contains the data to write.
+ * @param size: The number of bytes to write.
+ */
+ int multiByteWrite(char startAddress, char* ptr_data, int size);
+
+};
+
+#endif /* ADXL345_I2C_H */
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP085.cpp Wed Jul 17 18:50:28 2013 +0000
@@ -0,0 +1,171 @@
+/*
+ * BMP085.cpp
+ *
+ * Created on: 13 juil. 2013
+ * Author: Vincent
+ */
+
+#include "BMP085.h"
+#include <new>
+
+BMP085::BMP085(PinName sda, PinName scl) : i2c_(*reinterpret_cast<I2C*>(i2cRaw))
+{
+ // Placement new to avoid additional heap memory allocation.
+ new(i2cRaw) I2C(sda, scl);
+
+ pressure = 101300;
+ temperature = 298;
+}
+
+BMP085::~BMP085()
+{
+ // If the I2C object is initialized in the buffer in this object, call destructor of it.
+ if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw))
+ reinterpret_cast<I2C*>(&i2cRaw)->~I2C();
+}
+
+// oversampling setting
+// 0 = ultra low power
+// 1 = standard
+// 2 = high
+// 3 = ultra high resolution
+const unsigned char oversampling_setting = 3; //oversampling for measurement
+const unsigned char pressure_conversiontime[4] = {4.5, 7.5, 13.5, 25.5 }; // delays for oversampling settings 0, 1, 2 and 3
+
+// sensor registers from the BOSCH BMP085 datasheet
+short ac1;
+short ac2;
+short ac3;
+unsigned short ac4;
+unsigned short ac5;
+unsigned short ac6;
+short b1;
+short b2;
+short mb;
+short mc;
+short md;
+
+void BMP085::writeRegister(unsigned char r, unsigned char v)
+{
+ char cmd1[2];
+ cmd1[0] = r;
+ cmd1[1] = v;
+ i2c_.write(I2C_ADDRESS,cmd1, 2);
+}
+
+// read a 16 bit register
+int BMP085::readIntRegister(unsigned char r)
+{
+ char cmd1[2];
+ char cmd2[1];
+ //unsigned char msb, lsb;
+ cmd2[0] = r;
+ i2c_.write(I2C_ADDRESS,cmd2, 1);
+ i2c_.read(I2C_ADDRESS, cmd1, 2);
+ return (((int)cmd1[0]<<8) | ((int)cmd1[1]));
+}
+
+// read uncompensated temperature value
+unsigned int BMP085::readUT() {
+ writeRegister(0xf4,0x2e);
+ wait(0.0045); // the datasheet suggests 4.5 ms
+ return readIntRegister(0xf6);
+
+}
+
+// read uncompensated pressure value
+long BMP085::readUP() {
+ writeRegister(0xf4,0x34+(oversampling_setting<<6));
+ wait(pressure_conversiontime[oversampling_setting]*0.001);
+
+ //unsigned char msb, lsb, xlsb;
+ char cmd1[3];
+ char cmd0[1];
+ cmd0[0] = 0xf6;
+ i2c_.write(I2C_ADDRESS,cmd0, 1);
+ i2c_.read(I2C_ADDRESS, cmd1, 3);
+ return (((long)cmd1[0]<<16) | ((long)cmd1[1]<<8) | ((long)cmd1[2])) >>(8-oversampling_setting);
+
+}
+
+// Below there are the utility functions to get data from the sensor.
+
+// read temperature and pressure from sensor
+void BMP085::readSensor() {
+
+ long ut= readUT();
+ long up = readUP();
+
+
+ int x1, x2, x3, b3, b5, b6, p;
+ unsigned int b4, b7;
+ //calculate true temperature
+ x1 = ((long)ut - ac6) * ac5 >> 15;
+ x2 = ((long) mc << 11) / (x1 + md);
+ b5 = x1 + x2;
+ temperature = (b5 + 8) >> 4;
+
+ //calculate true pressure
+ b6 = b5 - 4000;
+ x1 = (b2 * (b6 * b6 >> 12)) >> 11;
+ x2 = ac2 * b6 >> 11;
+ x3 = x1 + x2;
+
+ if (oversampling_setting == 3) b3 = ((int32_t) ac1 * 4 + x3 + 2) << 1;
+ if (oversampling_setting == 2) b3 = ((int32_t) ac1 * 4 + x3 + 2);
+ if (oversampling_setting == 1) b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 1;
+ if (oversampling_setting == 0) b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2;
+
+ x1 = ac3 * b6 >> 13;
+ x2 = (b1 * (b6 * b6 >> 12)) >> 16;
+ x3 = ((x1 + x2) + 2) >> 2;
+ b4 = (ac4 * (unsigned long) (x3 + 32768)) >> 15;
+ b7 = ((unsigned long) up - b3) * (50000 >> oversampling_setting);
+ p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
+
+ x1 = (p >> 8) * (p >> 8);
+ x1 = (x1 * 3038) >> 16;
+ x2 = (-7357 * p) >> 16;
+ pressure = p + ((x1 + x2 + 3791) >> 4);
+}
+
+void BMP085::getCalibrationData() {
+ ac1 = readIntRegister(0xAA);
+ ac2 = readIntRegister(0xAC);
+ ac3 = readIntRegister(0xAE);
+ ac4 = readIntRegister(0xB0);
+ ac5 = readIntRegister(0xB2);
+ ac6 = readIntRegister(0xB4);
+ b1 = readIntRegister(0xB6);
+ b2 = readIntRegister(0xB8);
+ mb = readIntRegister(0xBA);
+ mc = readIntRegister(0xBC);
+ md = readIntRegister(0xBE);
+}
+
+float BMP085::press(void)
+{
+ return(pressure);
+}
+
+float BMP085::temp(void)
+{
+ return(temperature);
+}
+
+float BMP085::altitud(void)
+{
+ //press();
+ // temp();
+ float To=298;
+ float ho=0;
+ float Po=101325;
+ //ecuacion
+ float c=(To-0.0065*ho);
+ float e=(pressure/Po);
+ float d=exp(0.19082*log(e));
+ float b=c*d;
+ float alt=153.84615*(To-b);
+ return(alt);
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP085.h Wed Jul 17 18:50:28 2013 +0000
@@ -0,0 +1,67 @@
+/*
+ * BMP085.h
+ *
+ * Created on: 13 juil. 2013
+ * Author: Vincent
+ */
+
+#ifndef BMP085_H_
+#define BMP085_H_
+
+#include "mbed.h"
+
+class BMP085{
+
+public :
+
+ static const int I2C_ADDRESS = 0xEE; // sensor address
+
+ BMP085(PinName sda, PinName scl);
+
+
+ BMP085(I2C &i2c) : i2c_(i2c) {
+ //init();
+ }
+
+ ~BMP085();
+
+ void writeRegister(unsigned char r, unsigned char v);
+
+ // read a 16 bit register
+ int readIntRegister(unsigned char r);
+
+ // read uncompensated temperature value
+ unsigned int readUT();
+
+
+ // read uncompensated pressure value
+ long readUP();
+
+ // Below there are the utility functions to get data from the sensor.
+
+ // read temperature and pressure from sensor
+ void readSensor();
+
+ void getCalibrationData();
+
+ float press(void);
+
+ float temp(void);
+
+ float altitud(void);
+
+
+private :
+
+ I2C &i2c_;
+ // variables to keep the values
+ float temperature;
+ float pressure;
+
+
+ char i2cRaw[sizeof(I2C)];
+};
+
+
+#endif /* BMP085_H_ */
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HK10DOF.cpp Wed Jul 17 18:50:28 2013 +0000
@@ -0,0 +1,550 @@
+/*
+FreeIMU.cpp - A libre and easy to use orientation sensing library for Arduino
+Copyright (C) 2011-2012 Fabio Varesano <fabio at varesano dot net>
+ported for HobbyKing's 10DOF stick on a mbed platform by Aloïs Wolff(wolffalois@gmail.com)
+
+Development of this code has been supported by the Department of Computer Science,
+Universita' degli Studi di Torino, Italy within the Piemonte Project
+http://www.piemonte.di.unito.it/
+
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the version 3 GNU General Public License as
+published by the Free Software Foundation.
+
+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/>.
+
+*/
+
+
+//#define DEBUG
+
+
+#include "HK10DOF.h"
+#include "helper_3dmath.h"
+#include <math.h>
+
+// #include "WireUtils.h"
+//#include "DebugUtils.h"
+//#include "vector_math.h"
+
+#define M_PI 3.1415926535897932384626433832795
+
+HK10DOF::HK10DOF(PinName sda, PinName scl) : acc(sda,scl), magn(sda,scl),gyro(sda,scl), baro(sda,scl),pc(USBTX,USBRX)
+{
+
+ pc.baud(921600);
+
+ // initialize quaternion
+ q0 = 1.0f;
+ q1 = 0.0f;
+ q2 = 0.0f;
+ q3 = 0.0f;
+ exInt = 0.0;
+ eyInt = 0.0;
+ ezInt = 0.0;
+ twoKp = twoKpDef;
+ twoKi = twoKiDef;
+ integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f;
+
+
+#ifndef CALIBRATION_H
+ // initialize scale factors to neutral values
+ acc_scale_x = 1;
+ acc_scale_y = 1;
+ acc_scale_z = 1;
+ magn_scale_x = 1;
+ magn_scale_y = 1;
+ magn_scale_z = 1;
+#else
+ // get values from global variables of same name defined in calibration.h
+ acc_off_x = ::acc_off_x;
+ acc_off_y = ::acc_off_y;
+ acc_off_z = ::acc_off_z;
+ acc_scale_x = ::acc_scale_x;
+ acc_scale_y = ::acc_scale_y;
+ acc_scale_z = ::acc_scale_z;
+ magn_off_x = ::magn_off_x;
+ magn_off_y = ::magn_off_y;
+ magn_off_z = ::magn_off_z;
+ magn_scale_x = ::magn_scale_x;
+ magn_scale_y = ::magn_scale_y;
+ magn_scale_z = ::magn_scale_z;
+#endif
+}
+
+
+
+
+/**
+ * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration
+*/
+
+void HK10DOF::init(bool fastmode)
+{
+
+
+ //Sensors Initialization
+ pc.printf("Initializing sensors...\n\r");
+ magn.init();
+ baro.getCalibrationData();
+ acc.setPowerControl(0x00);
+ wait_ms(10);
+ acc.setDataFormatControl(0x0B);
+ wait_ms(10);
+ acc.setPowerControl(MeasurementMode);
+ wait_ms(10);
+ pc.printf("Sensors OK!\n\r");
+
+ update.start();
+ int dt_us=0;
+ pc.printf("ZeroGyro\n\r");
+ // zero gyro
+ //zeroGyro();
+ gyro.zeroCalibrate(128,5);
+ pc.printf("ZeroGyro OK, CalLoad...\n\r");
+ // load calibration from eeprom
+ calLoad();
+
+}
+
+void HK10DOF::calLoad()
+{
+
+
+ acc_off_x = 0;
+ acc_off_y = 0;
+ acc_off_z = 0;
+ acc_scale_x = 1;
+ acc_scale_y = 1;
+ acc_scale_z = 1;
+
+ magn_off_x = 0;
+ magn_off_y = 0;
+ magn_off_z = 0;
+ magn_scale_x = 1;
+ magn_scale_y = 1;
+ magn_scale_z = 1;
+}
+
+void HK10DOF::getRawValues(int16_t * raw_values)
+{
+ int16_t raw3[3];
+ int raw[3];
+
+ pc.printf("GetRaw IN \n\r");
+
+ acc.getOutput(&raw_values[0], &raw_values[1], &raw_values[2]);
+ pc.printf("GotACC\n\r");
+
+ gyro.read(raw);
+
+ raw_values[3]=(int16_t)raw[0];
+ raw_values[4]=(int16_t)raw[1];
+ raw_values[5]=(int16_t)raw[2];
+ pc.printf("GotGYRO\n\r");
+
+
+ magn.getXYZ(raw3);
+ raw_values[6]=raw3[0];
+ raw_values[7]=raw3[1];
+ raw_values[8]=raw3[2];
+ pc.printf("GotMAG\n\r");
+
+
+
+ baro.readSensor();
+ raw_values[9]=(int16_t)baro.temp();
+ raw_values[10]=(int16_t)baro.press();
+ pc.printf("GotBARO\n\r");
+
+
+}
+
+
+/**
+ * Populates values with calibrated readings from the sensors
+*/
+void HK10DOF::getValues(float * values)
+{
+
+ int16_t accval[3];
+ float gyrval[3];
+ acc.getOutput(accval);
+ values[0] = (float) accval[0];
+ values[1] = (float) accval[1];
+ values[2] = (float) accval[2];
+
+ gyro.readFin(gyrval);
+ values[3] = gyrval[0];
+ values[4] = gyrval[1];
+ values[5] = gyrval[2];
+
+ magn.getXYZ(accval);
+ values[6]=accval[0];
+ values[7]=accval[1];
+ values[8]=accval[2];
+
+
+#warning Accelerometer calibration active: have you calibrated your device?
+ // remove offsets and scale accelerometer (calibration)
+ values[0] = (values[0] - acc_off_x) / acc_scale_x;
+ values[1] = (values[1] - acc_off_y) / acc_scale_y;
+ values[2] = (values[2] - acc_off_z) / acc_scale_z;
+
+ // calibration
+#warning Magnetometer calibration active: have you calibrated your device?
+ values[6] = (values[6] - magn_off_x) / magn_scale_x;
+ values[7] = (values[7] - magn_off_y) / magn_scale_y;
+ values[8] = (values[8] - magn_off_z) / magn_scale_z;
+
+
+
+}
+
+
+/**
+ * Computes gyro offsets
+*/
+void HK10DOF::zeroGyro()
+{
+ const int totSamples = 3;
+ int16_t raw[11];
+ float tmpOffsets[] = {0,0,0};
+
+ for (int i = 0; i < totSamples; i++) {
+ getRawValues(raw);
+ tmpOffsets[0] += raw[3];
+ tmpOffsets[1] += raw[4];
+ tmpOffsets[2] += raw[5];
+ }
+
+ gyro_off_x = tmpOffsets[0] / totSamples;
+ gyro_off_y = tmpOffsets[1] / totSamples;
+ gyro_off_z = tmpOffsets[2] / totSamples;
+}
+
+
+/**
+ * Quaternion implementation of the 'DCM filter' [Mayhony et al]. Incorporates the magnetic distortion
+ * compensation algorithms from Sebastian Madgwick's filter which eliminates the need for a reference
+ * direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
+ * axis only.
+ *
+ * @see: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+*/
+//#if IS_9DOM()
+void HK10DOF::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
+{
+
+ float recipNorm;
+ float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+ float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
+ float qa, qb, qc;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ q0q0 = q0 * q0;
+ q0q1 = q0 * q1;
+ q0q2 = q0 * q2;
+ q0q3 = q0 * q3;
+ q1q1 = q1 * q1;
+ q1q2 = q1 * q2;
+ q1q3 = q1 * q3;
+ q2q2 = q2 * q2;
+ q2q3 = q2 * q3;
+ q3q3 = q3 * q3;
+
+ // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation)
+ if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f)) {
+ float hx, hy, bx, bz;
+ float halfwx, halfwy, halfwz;
+
+ // Normalise magnetometer measurement
+ recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+ mx *= recipNorm;
+ my *= recipNorm;
+ mz *= recipNorm;
+
+ // Reference direction of Earth's magnetic field
+ hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
+ hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
+ bx = sqrt(hx * hx + hy * hy);
+ bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
+
+ // Estimated direction of magnetic field
+ halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
+ halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
+ halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
+
+ // Error is sum of cross product between estimated direction and measured direction of field vectors
+ halfex = (my * halfwz - mz * halfwy);
+ halfey = (mz * halfwx - mx * halfwz);
+ halfez = (mx * halfwy - my * halfwx);
+ }
+
+
+ // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+ if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
+ float halfvx, halfvy, halfvz;
+
+ // Normalise accelerometer measurement
+ recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+ ax *= recipNorm;
+ ay *= recipNorm;
+ az *= recipNorm;
+
+ // Estimated direction of gravity
+ halfvx = q1q3 - q0q2;
+ halfvy = q0q1 + q2q3;
+ halfvz = q0q0 - 0.5f + q3q3;
+
+ // Error is sum of cross product between estimated direction and measured direction of field vectors
+ halfex += (ay * halfvz - az * halfvy);
+ halfey += (az * halfvx - ax * halfvz);
+ halfez += (ax * halfvy - ay * halfvx);
+ }
+
+ // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
+ if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
+ // Compute and apply integral feedback if enabled
+ if(twoKi > 0.0f) {
+ integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
+ integralFBy += twoKi * halfey * (1.0f / sampleFreq);
+ integralFBz += twoKi * halfez * (1.0f / sampleFreq);
+ gx += integralFBx; // apply integral feedback
+ gy += integralFBy;
+ gz += integralFBz;
+ } else {
+ integralFBx = 0.0f; // prevent integral windup
+ integralFBy = 0.0f;
+ integralFBz = 0.0f;
+ }
+
+ // Apply proportional feedback
+ gx += twoKp * halfex;
+ gy += twoKp * halfey;
+ gz += twoKp * halfez;
+ }
+
+ // Integrate rate of change of quaternion
+ gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
+ gy *= (0.5f * (1.0f / sampleFreq));
+ gz *= (0.5f * (1.0f / sampleFreq));
+ qa = q0;
+ qb = q1;
+ qc = q2;
+ q0 += (-qb * gx - qc * gy - q3 * gz);
+ q1 += (qa * gx + qc * gz - q3 * gy);
+ q2 += (qa * gy - qb * gz + q3 * gx);
+ q3 += (qa * gz + qb * gy - qc * gx);
+
+ // Normalise quaternion
+ recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+ q0 *= recipNorm;
+ q1 *= recipNorm;
+ q2 *= recipNorm;
+ q3 *= recipNorm;
+}
+
+
+/**
+ * Populates array q with a quaternion representing the IMU orientation with respect to the Earth
+ *
+ * @param q the quaternion to populate
+*/
+void HK10DOF::getQ(float * q)
+{
+ float val[9];
+ getValues(val);
+ /*
+ DEBUG_PRINT(val[3] * M_PI/180);
+ DEBUG_PRINT(val[4] * M_PI/180);
+ DEBUG_PRINT(val[5] * M_PI/180);
+ DEBUG_PRINT(val[0]);
+ DEBUG_PRINT(val[1]);
+ DEBUG_PRINT(val[2]);
+ DEBUG_PRINT(val[6]);
+ DEBUG_PRINT(val[7]);
+ DEBUG_PRINT(val[8]);
+ */
+
+ dt_us=update.read_us();
+ sampleFreq = 1.0 / ((dt_us) / 1000000.0);
+ update.reset();
+
+
+
+ // gyro values are expressed in deg/sec, the * M_PI/180 will convert it to radians/sec
+ AHRSupdate(val[3] * M_PI/180, val[4] * M_PI/180, val[5] * M_PI/180, val[0], val[1], val[2], val[6], val[7], val[8]);
+
+
+
+ q[0] = q0;
+ q[1] = q1;
+ q[2] = q2;
+ q[3] = q3;
+}
+
+
+
+const float def_sea_press = 1013.25;
+
+/**
+ * Returns an altitude estimate from baromether readings only using sea_press as current sea level pressure
+*/
+float HK10DOF::getBaroAlt(float sea_press)
+{
+ //float temp,press,res;
+ baro.readSensor();
+ //temp=(float)baro.temp();
+ //press=(float)baro.press();
+
+
+
+ return baro.altitud();
+ //return(temp,press);
+}
+
+/**
+ * Returns an altitude estimate from baromether readings only using a default sea level pressure
+*/
+float HK10DOF::getBaroAlt()
+{
+ return getBaroAlt(def_sea_press);
+}
+
+
+/**
+ * Compensates the accelerometer readings in the 3D vector acc expressed in the sensor frame for gravity
+ * @param acc the accelerometer readings to compensate for gravity
+ * @param q the quaternion orientation of the sensor board with respect to the world
+*/
+void HK10DOF::gravityCompensateAcc(float * acc, float * q)
+{
+ float g[3];
+
+ // get expected direction of gravity in the sensor frame
+ g[0] = 2 * (q[1] * q[3] - q[0] * q[2]);
+ g[1] = 2 * (q[0] * q[1] + q[2] * q[3]);
+ g[2] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
+
+ // compensate accelerometer readings with the expected direction of gravity
+ acc[0] = acc[0] - g[0];
+ acc[1] = acc[1] - g[1];
+ acc[2] = acc[2] - g[2];
+}
+
+
+
+/**
+ * Returns the Euler angles in radians defined in the Aerospace sequence.
+ * See Sebastian O.H. Madwick report "An efficient orientation filter for
+ * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
+ *
+ * @param angles three floats array which will be populated by the Euler angles in radians
+*/
+void HK10DOF::getEulerRad(float * angles)
+{
+ float q[4]; // quaternion
+ getQ(q);
+ angles[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); // psi
+ angles[1] = -asin(2 * q[1] * q[3] + 2 * q[0] * q[2]); // theta
+ angles[2] = atan2(2 * q[2] * q[3] - 2 * q[0] * q[1], 2 * q[0] * q[0] + 2 * q[3] * q[3] - 1); // phi
+}
+
+
+/**
+ * Returns the Euler angles in degrees defined with the Aerospace sequence.
+ * See Sebastian O.H. Madwick report "An efficient orientation filter for
+ * inertial and intertial/magnetic sensor arrays" Chapter 2 Quaternion representation
+ *
+ * @param angles three floats array which will be populated by the Euler angles in degrees
+*/
+void HK10DOF::getEuler(float * angles)
+{
+ getEulerRad(angles);
+ arr3_rad_to_deg(angles);
+}
+
+
+/**
+ * Returns the yaw pitch and roll angles, respectively defined as the angles in radians between
+ * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
+ * and the Earth ground plane and the IMU Y axis.
+ *
+ * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
+ * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see HK10DOF::getEuler
+ *
+ * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in radians
+*/
+void HK10DOF::getYawPitchRollRad(float * ypr)
+{
+ float q[4]; // quaternion
+ float gx, gy, gz; // estimated gravity direction
+ getQ(q);
+
+ gx = 2 * (q[1]*q[3] - q[0]*q[2]);
+ gy = 2 * (q[0]*q[1] + q[2]*q[3]);
+ gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
+
+ ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1);
+ ypr[1] = atan(gx / sqrt(gy*gy + gz*gz));
+ ypr[2] = atan(gy / sqrt(gx*gx + gz*gz));
+}
+
+
+/**
+ * Returns the yaw pitch and roll angles, respectively defined as the angles in degrees between
+ * the Earth North and the IMU X axis (yaw), the Earth ground plane and the IMU X axis (pitch)
+ * and the Earth ground plane and the IMU Y axis.
+ *
+ * @note This is not an Euler representation: the rotations aren't consecutive rotations but only
+ * angles from Earth and the IMU. For Euler representation Yaw, Pitch and Roll see HK10DOF::getEuler
+ *
+ * @param ypr three floats array which will be populated by Yaw, Pitch and Roll angles in degrees
+*/
+void HK10DOF::getYawPitchRoll(float * ypr)
+{
+ getYawPitchRollRad(ypr);
+ arr3_rad_to_deg(ypr);
+}
+
+
+/**
+ * Converts a 3 elements array arr of angles expressed in radians into degrees
+*/
+void arr3_rad_to_deg(float * arr)
+{
+ arr[0] *= 180/M_PI;
+ arr[1] *= 180/M_PI;
+ arr[2] *= 180/M_PI;
+}
+
+
+/**
+ * Fast inverse square root implementation
+ * @see http://en.wikipedia.org/wiki/Fast_inverse_square_root
+*/
+float invSqrt(float number)
+{
+ volatile long i;
+ volatile float x, y;
+ volatile const float f = 1.5F;
+
+ x = number * 0.5F;
+ y = number;
+ i = * ( long * ) &y;
+ i = 0x5f375a86 - ( i >> 1 );
+ y = * ( float * ) &i;
+ y = y * ( f - ( x * y * y ) );
+ return y;
+}
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HK10DOF.h Wed Jul 17 18:50:28 2013 +0000
@@ -0,0 +1,171 @@
+/*
+FreeIMU.h - A libre and easy to use orientation sensing library for Arduino
+Copyright (C) 2011 Fabio Varesano <fabio at varesano dot net>
+
+Development of this code has been supported by the Department of Computer Science,
+Universita' degli Studi di Torino, Italy within the Piemonte Project
+http://www.piemonte.di.unito.it/
+
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the version 3 GNU General Public License as
+published by the Free Software Foundation.
+
+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 HK10DOF_h
+#define HK10DOF_h
+
+// Uncomment the appropriated version of FreeIMU you are using
+//#define FREEIMU_v01
+//#define FREEIMU_v02
+//#define FREEIMU_v03
+//#define FREEIMU_v035
+//#define FREEIMU_v035_MS
+//#define FREEIMU_v035_BMP
+#define FREEIMU_v04
+
+
+
+#define FREEIMU_LIB_VERSION "20121122"
+
+#define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net"
+
+#if F_CPU == 16000000L
+ #define FREEIMU_FREQ "16 MHz"
+#elif F_CPU == 8000000L
+ #define FREEIMU_FREQ "8 MHz"
+#endif
+
+
+// board IDs
+
+
+#define FREEIMU_ID "Zboub"
+
+//#define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050))
+#define IS_9DOM()
+//#define HAS_AXIS_ALIGNED() (defined(FREEIMU_v01) || defined(FREEIMU_v02) || defined(FREEIMU_v03) || defined(FREEIMU_v035) || defined(FREEIMU_v035_MS) || defined(FREEIMU_v035_BMP) || defined(FREEIMU_v04) || defined(SEN_10121) || defined(SEN_10736))
+
+
+
+
+#include "mbed.h"
+
+//#include "calibration.h"
+/*
+#ifndef CALIBRATION_H
+#include <EEPROM.h>
+#endif
+#define FREEIMU_EEPROM_BASE 0x0A
+#define FREEIMU_EEPROM_SIGNATURE 0x19
+*/
+
+//include des biblis des senseurs
+
+ #include "ADXL345_I2C.h"
+ #include "HMC5883L.h"
+ #include "BMP085.h"
+ #include "L3G4200D.h"
+
+ // default I2C 7-bit addresses of the sensors
+ #define FIMU_ACC_ADDR ADXL345_ADDR_ALT_LOW // SDO connected to GND
+//#define FIMU_ADXL345_DEF_ADDR ADXL345_ADDR_ALT_HIGH // SDO connected to GND
+
+#define FIMU_BMA180_DEF_ADDR BMA180_ADDRESS_SDO_LOW
+#define FIMU_ITG3200_DEF_ADDR ITG3200_ADDR_AD0_LOW // AD0 connected to GND
+// HMC5843 address is fixed so don't bother to define it
+
+/*
+#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
+#define twoKiDef (2.0f * 0.1f) // 2 * integral gain
+*/
+#define twoKpDef (2.0f * 5.0f) // 2 * proportional gain
+#define twoKiDef (2.0f * 0.1f) // 2 * integral gain
+
+
+#ifndef cbi
+#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
+#endif
+
+class HK10DOF
+{
+ public:
+ HK10DOF(PinName sda, PinName scl);
+ //I2C i2c_;
+
+ //void init();
+ //void init(bool fastmode);
+ //void init(int accgyro_addr, bool fastmode);
+ void init(bool fastmode);
+ #ifndef CALIBRATION_H
+ void calLoad();
+ #endif
+ void zeroGyro();
+ void getRawValues(int16_t * raw_values);
+ void getValues(float * values);
+ void getQ(float * q);
+ void getEuler(float * angles);
+ void getYawPitchRoll(float * ypr);
+ void getEulerRad(float * angles);
+ void getYawPitchRollRad(float * ypr);
+
+ float getBaroAlt();
+ float getBaroAlt(float sea_press);
+ //float getEstimatedAlt();
+ //float getEstimatedAlt(float sea_press);
+
+ void gravityCompensateAcc(float * acc, float * q);
+
+ // we make them public so that users can interact directly with device classes
+
+ ADXL345_I2C acc;
+ HMC5883L magn;
+ BMP085 baro;
+ L3G4200D gyro;
+ Serial pc;
+
+
+
+
+ int* raw_acc, raw_gyro, raw_magn;
+ // calibration parameters
+ int16_t gyro_off_x, gyro_off_y, gyro_off_z;
+ int16_t acc_off_x, acc_off_y, acc_off_z, magn_off_x, magn_off_y, magn_off_z;
+ float acc_scale_x, acc_scale_y, acc_scale_z, magn_scale_x, magn_scale_y, magn_scale_z;
+
+ private:
+ Timer update;
+ int dt_us;
+
+ void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+
+ //void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az); for 6DOF sensors
+
+ //float q0, q1, q2, q3; // quaternion elements representing the estimated orientation
+ float iq0, iq1, iq2, iq3;
+ float exInt, eyInt, ezInt; // scaled integral error
+ volatile float twoKp; // 2 * proportional gain (Kp)
+ volatile float twoKi; // 2 * integral gain (Ki)
+ volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame
+ volatile float integralFBx, integralFBy, integralFBz;
+ //unsigned long lastUpdate, now; // sample period expressed in milliseconds
+ float sampleFreq; // half the sample period expressed in seconds
+
+};
+
+float invSqrt(float number);
+void arr3_rad_to_deg(float * arr);
+
+
+
+#endif // FreeIMU_h
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5883L.cpp Wed Jul 17 18:50:28 2013 +0000
@@ -0,0 +1,147 @@
+/*
+ * @file HMC5883L.cpp
+ * @author Tyler Weaver
+ *
+ * @section LICENSE
+ *
+ * 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.
+ *
+ * @section DESCRIPTION
+ *
+ * HMC5883L 3-Axis Digital Compas IC
+ * For use with the Sparkfun 9 Degrees of Freedom - Sensor Stick
+ *
+ * Datasheet:
+ *
+ * http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/Magneto/HMC5883L-FDS.pdf
+ */
+
+#include "HMC5883L.h"
+#include <new>
+
+HMC5883L::HMC5883L(PinName sda, PinName scl) : i2c_(*reinterpret_cast<I2C*>(i2cRaw))
+{
+ // Placement new to avoid additional heap memory allocation.
+ new(i2cRaw) I2C(sda, scl);
+
+ init();
+}
+
+HMC5883L::~HMC5883L()
+{
+ // If the I2C object is initialized in the buffer in this object, call destructor of it.
+ if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw))
+ reinterpret_cast<I2C*>(&i2cRaw)->~I2C();
+}
+
+void HMC5883L::init()
+{
+ // init - configure your setup here
+ setConfigurationA(AVG8_SAMPLES | OUTPUT_RATE_15); // 8 sample average, 15Hz, normal mode
+ setConfigurationB(0x20); // default
+ setMode(CONTINUOUS_MODE); // continuous sample mode
+}
+
+void HMC5883L::setConfigurationA(char config)
+{
+ char cmd[2];
+ cmd[0] = CONFIG_A_REG; // register a address
+ cmd[1] = config;
+
+ i2c_.write(I2C_ADDRESS, cmd, 2);
+}
+
+void HMC5883L::setConfigurationB(char config)
+{
+ char cmd[2];
+ cmd[0] = CONFIG_B_REG; // register b address
+ cmd[1] = config;
+
+ i2c_.write(I2C_ADDRESS, cmd, 2);
+}
+
+char HMC5883L::getConfigurationA()
+{
+ char cmd[2];
+ cmd[0] = CONFIG_A_REG; // register a address
+ i2c_.write(I2C_ADDRESS, cmd, 1, true);
+ i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
+ return cmd[1];
+}
+
+char HMC5883L::getConfigurationB()
+{
+ char cmd[2];
+ cmd[0] = CONFIG_A_REG; // register b address
+ i2c_.write(I2C_ADDRESS, cmd, 1, true);
+ i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
+ return cmd[1];
+}
+
+void HMC5883L::setMode(char mode = SINGLE_MODE)
+{
+ char cmd[2];
+ cmd[0] = MODE_REG; // mode register address
+ cmd[1] = mode;
+ i2c_.write(I2C_ADDRESS,cmd,2);
+}
+
+char HMC5883L::getMode()
+{
+ char cmd[2];
+ cmd[0] = MODE_REG; // mode register
+ i2c_.write(I2C_ADDRESS, cmd, 1, true);
+ i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
+ return cmd[1];
+}
+
+char HMC5883L::getStatus()
+{
+ char cmd[2];
+ cmd[0] = STATUS_REG; // status register
+ i2c_.write(I2C_ADDRESS, cmd, 1, true);
+ i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
+ return cmd[1];
+}
+
+void HMC5883L::getXYZ(int16_t output[3])
+{
+ char cmd[2];
+ char data[6];
+ cmd[0] = 0x03; // starting point for reading
+ i2c_.write(I2C_ADDRESS, cmd, 1, true); // set the pointer to the start of x
+ i2c_.read(I2C_ADDRESS, data, 6, false);
+
+ for(int i = 0; i < 3; i++) // fill the output variables
+ output[i] = int16_t(((unsigned char)data[i*2] << 8) | (unsigned char)data[i*2+1]);
+}
+
+double HMC5883L::getHeadingXY()
+{
+ int16_t raw_data[3];
+ getXYZ(raw_data);
+ double heading = atan2(static_cast<double>(raw_data[2]), static_cast<double>(raw_data[0])); // heading = arctan(Y/X)
+
+ // TODO: declenation angle compensation
+
+ if(heading < 0.0) // fix sign
+ heading += PI2;
+
+ if(heading > PI2) // fix overflow
+ heading -= PI2;
+
+ return heading;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5883L.h Wed Jul 17 18:50:28 2013 +0000
@@ -0,0 +1,246 @@
+/*
+ * @file HMC5883L.h
+ * @author Tyler Weaver
+ *
+ * @section LICENSE
+ *
+ * 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.
+ *
+ * @section DESCRIPTION
+ *
+ * HMC5883L 3-Axis Digital Compas IC
+ * For use with the Sparkfun 9 Degrees of Freedom - Sensor Stick
+ *
+ * Datasheet:
+ *
+ * http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/Magneto/HMC5883L-FDS.pdf
+ */
+
+#ifndef HMC5883L_H
+#define HMC5883L_H
+
+#include "mbed.h"
+
+/*
+* Defines
+*/
+
+//-----------
+// Registers
+//-----------
+#define CONFIG_A_REG 0x00
+#define CONFIG_B_REG 0x01
+#define MODE_REG 0x02
+#define OUTPUT_REG 0x03
+#define STATUS_REG 0x09
+
+// configuration register a
+#define AVG1_SAMPLES 0x00
+#define AVG2_SAMPLES 0x20
+#define AVG4_SAMPLES 0x80
+#define AVG8_SAMPLES 0xC0
+
+#define OUTPUT_RATE_0_75 0x00
+#define OUTPUT_RATE_1_5 0x04
+#define OUTPUT_RATE_3 0x08
+#define OUTPUT_RATE_7_5 0x0C
+#define OUTPUT_RATE_15 0x10
+#define OUTPUT_RATE_30 0x14
+#define OUTPUT_RATE_75 0x18
+
+#define NORMAL_MEASUREMENT 0x00
+#define POSITIVE_BIAS 0x01
+#define NEGATIVE_BIAS 0x02
+
+// mode register
+#define CONTINUOUS_MODE 0x00
+#define SINGLE_MODE 0x01
+#define IDLE_MODE 0x02
+
+// status register
+#define STATUS_LOCK 0x02
+#define STATUS_READY 0x01
+
+// Utility
+#ifndef M_PI
+#define M_PI 3.1415926535897932384626433832795
+#endif
+
+#define PI2 (2*M_PI)
+#define RAD_TO_DEG (180.0/M_PI)
+#define DEG_TO_RAD (M_PI/180.0)
+
+/**
+ * The HMC5883L 3-Axis Digital Compass IC
+ */
+class HMC5883L
+{
+
+public:
+
+ /**
+ * The I2C address that can be passed directly to i2c object (it's already shifted 1 bit left).
+ */
+ static const int I2C_ADDRESS = 0x3D;
+
+ /**
+ * Constructor.
+ *
+ * Calls init function
+ *
+ * @param sda - mbed pin to use for the SDA I2C line.
+ * @param scl - mbed pin to use for the SCL I2C line.
+ */
+ HMC5883L(PinName sda, PinName scl);
+
+ /**
+ * Constructor that accepts external i2c interface object.
+ *
+ * Calls init function
+ *
+ * @param i2c The I2C interface object to use.
+ */
+ HMC5883L(I2C &i2c) : i2c_(i2c) {
+ init();
+ }
+
+ ~HMC5883L();
+
+ /**
+ * Initalize function called by all constructors.
+ *
+ * Place startup code in here.
+ */
+ void init();
+
+ /**
+ * Function for setting configuration register A
+ *
+ * Defined constants should be ored together to create value.
+ * Defualt is 0x10 - 1 Sample per output, 15Hz Data output rate, normal measurement mode
+ *
+ * Refer to datasheet for instructions for setting Configuration Register A.
+ *
+ * @param config the value to place in Configuration Register A
+ */
+ void setConfigurationA(char);
+
+ /**
+ * Function for retrieving the contents of configuration register A
+ *
+ * @returns Configuration Register A
+ */
+ char getConfigurationA();
+
+ /**
+ * Function for setting configuration register B
+ *
+ * Configuration Register B is for setting the device gain.
+ * Default value is 0x20
+ *
+ * Refer to datasheet for instructions for setting Configuration Register B
+ *
+ * @param config the value to place in Configuration Register B
+ */
+ void setConfigurationB(char);
+
+ /**
+ * Function for retrieving the contents of configuration register B
+ *
+ * @returns Configuration Register B
+ */
+ char getConfigurationB();
+
+ /**
+ * Funciton for setting the mode register
+ *
+ * Constants: CONTINUOUS_MODE, SINGLE_MODE, IDLE_MODE
+ *
+ * When you send a the Single-Measurement Mode instruction to the mode register
+ * a single measurement is made, the RDY bit is set in the status register,
+ * and the mode is placed in idle mode.
+ *
+ * When in Continous-Measurement Mode the device continuously performs measurements
+ * and places the results in teh data register. After being placed in this mode
+ * it takes two periods at the rate set in the data output rate before the first
+ * sample is avaliable.
+ *
+ * Refer to datasheet for more detailed instructions for setting the mode register.
+ *
+ * @param mode the value for setting in the Mode Register
+ */
+ void setMode(char);
+
+ /**
+ * Function for retrieving the contents of mode register
+ *
+ * @returns mode register
+ */
+ char getMode();
+
+ /**
+ * Function for retriaval of the raw data
+ *
+ * @param output buffer that is atleast 3 in length
+ */
+ void getXYZ(int16_t raw[3]);
+
+ /**
+ * Function for retrieving the contents of status register
+ *
+ * Bit1: LOCK, Bit0: RDY
+ *
+ * @returns status register
+ */
+ char getStatus();
+
+ /**
+ * Function for getting radian heading using 2-dimensional calculation.
+ *
+ * Compass must be held flat and away from an magnetic field generating
+ * devices such as cell phones and speakers.
+ *
+ * TODO: declenation angle compensation
+ *
+ * @returns heading in radians
+ */
+ double getHeadingXY();
+
+ /**
+ * Function for getting degree heading using 2-dimensional calculation.
+ *
+ * Compass must be held flat and away from an magnetic field generating
+ * devices such as cell phones and speakers.
+ *
+ * TODO: declenation angle compensation
+ *
+ * @returns heading in degrees
+ */
+ double getHeadingXYDeg() {
+ return (getHeadingXY() * RAD_TO_DEG);
+ }
+
+private:
+
+ I2C &i2c_;
+
+ /**
+ * The raw buffer for allocating I2C object in its own without heap memory.
+ */
+ char i2cRaw[sizeof(I2C)];
+};
+
+#endif // HMC5883L
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/L3G4200D.cpp Wed Jul 17 18:50:28 2013 +0000
@@ -0,0 +1,165 @@
+/**
+ * Copyright (c) 2011 Pololu Corporation. For more information, see
+ *
+ * http://www.pololu.com/
+ * http://forum.pololu.com/
+ *
+ * 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 "mbed.h"
+#include "L3G4200D.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
+// mbed I2C libraries take the 7-bit address shifted left 1 bit
+// #define GYR_ADDRESS (0xD2 >> 1)
+#define GYR_ADDRESS 0xD2
+
+// Public Methods //////////////////////////////////////////////////////////////
+
+// Constructor
+L3G4200D::L3G4200D(PinName sda, PinName scl):
+ _device(sda, scl)
+{
+ _device.frequency(400000);
+ // Turns on the L3G4200D's gyro and places it in normal mode.
+ // 0x0F = 0b00001111
+ // Normal power mode, all axes enabled
+ writeReg(L3G4200D_CTRL_REG1, 0x0F);
+ writeReg(L3G4200D_CTRL_REG4, 0x20); // 2000 dps full scale
+
+ setGains(1.0,1.0,1.0);
+ setOffsets(0.0,0.0,0.0);
+
+}
+
+// Writes a gyro register
+void L3G4200D::writeReg(byte reg, byte value)
+{
+ data[0] = reg;
+ data[1] = value;
+
+ _device.write(GYR_ADDRESS, data, 2);
+}
+
+// Reads a gyro register
+byte L3G4200D::readReg(byte reg)
+{
+ byte value = 0;
+
+ _device.write(GYR_ADDRESS, ®, 1);
+ _device.read(GYR_ADDRESS, &value, 1);
+
+ return value;
+}
+
+void L3G4200D::setGains(float _Xgain, float _Ygain, float _Zgain) {
+ gains[0] = _Xgain;
+ gains[1] = _Ygain;
+ gains[2] = _Zgain;
+}
+
+void L3G4200D::setOffsets(int _Xoffset, int _Yoffset, int _Zoffset) {
+ offsets[0] = _Xoffset;
+ offsets[1] = _Yoffset;
+ offsets[2] = _Zoffset;
+}
+
+void L3G4200D::setRevPolarity(bool _Xpol, bool _Ypol, bool _Zpol) {
+ polarities[0] = _Xpol ? -1 : 1;
+ polarities[1] = _Ypol ? -1 : 1;
+ polarities[2] = _Zpol ? -1 : 1;
+}
+
+
+void L3G4200D::zeroCalibrate(unsigned int totSamples, unsigned int sampleDelayMS) {
+ int xyz[3];
+ float tmpOffsets[] = {0,0,0};
+
+ for (int i = 0;i < totSamples;i++){
+ wait_ms(sampleDelayMS);
+ read(xyz);
+ tmpOffsets[0] += xyz[0];
+ tmpOffsets[1] += xyz[1];
+ tmpOffsets[2] += xyz[2];
+ }
+ setOffsets(-tmpOffsets[0] / totSamples, -tmpOffsets[1] / totSamples, -tmpOffsets[2] / totSamples);
+}
+
+
+// Reads the 3 gyro channels and stores them in vector g
+void L3G4200D::read(int *g)
+{
+ // assert the MSB of the address to get the gyro
+ // to do slave-transmit subaddress updating.
+ data[0] = L3G4200D_OUT_X_L | (1 << 7);
+ _device.write(GYR_ADDRESS, data, 1);
+
+// Wire.requestFrom(GYR_ADDRESS, 6);
+// while (Wire.available() < 6);
+
+ _device.read(GYR_ADDRESS, data, 6);
+
+ uint8_t xla = data[0];
+ uint8_t xha = data[1];
+ uint8_t yla = data[2];
+ uint8_t yha = data[3];
+ uint8_t zla = data[4];
+ uint8_t zha = data[5];
+
+ g[0] = (short) (yha << 8 | yla);
+ g[1] = (short) (xha << 8 | xla);
+ g[2] = (short) (zha << 8 | zla);
+}
+
+void L3G4200D::readRawCal(int *_GyroXYZ) {
+ read(_GyroXYZ);
+ _GyroXYZ[0] += offsets[0];
+ _GyroXYZ[1] += offsets[1];
+ _GyroXYZ[2] += offsets[2];
+}
+
+
+
+void L3G4200D::read3(int x, int y, int z) {
+ int* readings;
+ read(readings);
+
+ // each axis reading comes in 10 bit resolution, ie 2 bytes. Least Significat Byte first!!
+ // thus we are converting both bytes in to one int
+ x = readings[0];
+ y = readings[1];
+ z = readings[2];
+}
+
+void L3G4200D::readFin(float *_GyroXYZ){
+ int xyz[3];
+
+ readRawCal(xyz); // x,y,z will contain calibrated integer values from the sensor
+ _GyroXYZ[0] = xyz[0] / 14.375 * polarities[0] * gains[0];
+ _GyroXYZ[1] = xyz[1]/ 14.375 * polarities[1] * gains[1];
+ _GyroXYZ[2] = xyz[2] / 14.375 * polarities[2] * gains[2];
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/L3G4200D.h Wed Jul 17 18:50:28 2013 +0000
@@ -0,0 +1,119 @@
+/* Copyright (c) 2011 Pololu Corporation. For more information, see
+ *
+ * http://www.pololu.com/
+ * http://forum.pololu.com/
+ *
+ * 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"
+
+// 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
+
+typedef char byte;
+
+/** Interface library for the ST L3G4200D 3-axis gyro
+ *
+ * Ported from Pololu L3G4200D library for Arduino by
+ * Michael Shimniok http://bot-thoughts.com
+ *
+ * @code
+ * #include "mbed.h"
+ * #include "L3G4200D.h"
+ * L3G4200D gyro(p28, p27);
+ * ...
+ * int g[3];
+ * gyro.read(g);
+ * @endcode
+ */
+class L3G4200D
+{
+ public:
+ /** Create a new L3G4200D I2C interface
+ * @param sda is the pin for the I2C SDA line
+ * @param scl is the pin for the I2C SCL line
+ */
+ L3G4200D(PinName sda, PinName scl);
+
+ /** Read gyro values
+ * @param g Array containing x, y, and z gyro values
+ * @return g Array containing x, y, and z gyro values
+ */
+ void read(int *g);
+ void read3(int x, int y, int z);
+ void zeroCalibrate(unsigned int totSamples, unsigned int sampleDelayMS);
+ void readRawCal(int *_GyroXYZ);
+ void readFin(float *_GyroXYZ); // includes gain and offset
+
+ private:
+ float gains[3];
+ int offsets[3];
+ float polarities[3];
+
+ void setGains(float _Xgain, float _Ygain, float _Zgain);
+ void setOffsets(int _Xoffset, int _Yoffset, int _Zoffset);
+ void setRevPolarity(bool _Xpol, bool _Ypol, bool _Zpol); // true = Reversed false = default
+
+
+ byte data[6];
+ int _rates[3];
+ I2C _device;
+ void writeReg(byte reg, byte value);
+ byte readReg(byte reg);
+ void enableDefault(void);
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/helper_3dmath.h Wed Jul 17 18:50:28 2013 +0000
@@ -0,0 +1,216 @@
+// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
+// 6/5/2012 by Jeff Rowberg <jeff@rowberg.net>
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+// 2012-06-05 - add 3D math helper file to DMP6 example sketch
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2012 Jeff Rowberg
+
+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 _HELPER_3DMATH_H_
+#define _HELPER_3DMATH_H_
+
+class Quaternion {
+ public:
+ float w;
+ float x;
+ float y;
+ float z;
+
+ Quaternion() {
+ w = 1.0f;
+ x = 0.0f;
+ y = 0.0f;
+ z = 0.0f;
+ }
+
+ Quaternion(float nw, float nx, float ny, float nz) {
+ w = nw;
+ x = nx;
+ y = ny;
+ z = nz;
+ }
+
+ Quaternion getProduct(Quaternion q) {
+ // Quaternion multiplication is defined by:
+ // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
+ // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
+ // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
+ // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
+ return Quaternion(
+ w*q.w - x*q.x - y*q.y - z*q.z, // new w
+ w*q.x + x*q.w + y*q.z - z*q.y, // new x
+ w*q.y - x*q.z + y*q.w + z*q.x, // new y
+ w*q.z + x*q.y - y*q.x + z*q.w); // new z
+ }
+
+ Quaternion getConjugate() {
+ return Quaternion(w, -x, -y, -z);
+ }
+
+ float getMagnitude() {
+ return sqrt(w*w + x*x + y*y + z*z);
+ }
+
+ void normalize() {
+ float m = getMagnitude();
+ w /= m;
+ x /= m;
+ y /= m;
+ z /= m;
+ }
+
+ Quaternion getNormalized() {
+ Quaternion r(w, x, y, z);
+ r.normalize();
+ return r;
+ }
+};
+
+class VectorInt16 {
+ public:
+ int16_t x;
+ int16_t y;
+ int16_t z;
+
+ VectorInt16() {
+ x = 0;
+ y = 0;
+ z = 0;
+ }
+
+ VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
+ x = nx;
+ y = ny;
+ z = nz;
+ }
+
+ float getMagnitude() {
+ return sqrt((float)(x*x + y*y + z*z));
+ }
+
+ void normalize() {
+ float m = getMagnitude();
+ x /= m;
+ y /= m;
+ z /= m;
+ }
+
+ VectorInt16 getNormalized() {
+ VectorInt16 r(x, y, z);
+ r.normalize();
+ return r;
+ }
+
+ void rotate(Quaternion *q) {
+ // http://www.cprogramming.com/tutorial/3d/quaternions.html
+ // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
+ // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
+ // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
+
+ // P_out = q * P_in * conj(q)
+ // - P_out is the output vector
+ // - q is the orientation quaternion
+ // - P_in is the input vector (a*aReal)
+ // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
+ Quaternion p(0, x, y, z);
+
+ // quaternion multiplication: q * p, stored back in p
+ p = q -> getProduct(p);
+
+ // quaternion multiplication: p * conj(q), stored back in p
+ p = p.getProduct(q -> getConjugate());
+
+ // p quaternion is now [0, x', y', z']
+ x = p.x;
+ y = p.y;
+ z = p.z;
+ }
+
+ VectorInt16 getRotated(Quaternion *q) {
+ VectorInt16 r(x, y, z);
+ r.rotate(q);
+ return r;
+ }
+};
+
+class VectorFloat {
+ public:
+ float x;
+ float y;
+ float z;
+
+ VectorFloat() {
+ x = 0;
+ y = 0;
+ z = 0;
+ }
+
+ VectorFloat(float nx, float ny, float nz) {
+ x = nx;
+ y = ny;
+ z = nz;
+ }
+
+ float getMagnitude() {
+ return sqrt(x*x + y*y + z*z);
+ }
+
+ void normalize() {
+ float m = getMagnitude();
+ x /= m;
+ y /= m;
+ z /= m;
+ }
+
+ VectorFloat getNormalized() {
+ VectorFloat r(x, y, z);
+ r.normalize();
+ return r;
+ }
+
+ void rotate(Quaternion *q) {
+ Quaternion p(0, x, y, z);
+
+ // quaternion multiplication: q * p, stored back in p
+ p = q -> getProduct(p);
+
+ // quaternion multiplication: p * conj(q), stored back in p
+ p = p.getProduct(q -> getConjugate());
+
+ // p quaternion is now [0, x', y', z']
+ x = p.x;
+ y = p.y;
+ z = p.z;
+ }
+
+ VectorFloat getRotated(Quaternion *q) {
+ VectorFloat r(x, y, z);
+ r.rotate(q);
+ return r;
+ }
+};
+
+#endif /* _HELPER_3DMATH_H_ */