Revision 0:f5a5a0e5fefb, committed 2012-04-23
- Comitter:
- higedura
- Date:
- Mon Apr 23 14:59:48 2012 +0000
- Commit message:
Changed in this revision
diff -r 000000000000 -r f5a5a0e5fefb ADXL345_I2C.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345_I2C.cpp Mon Apr 23 14:59:48 2012 +0000
@@ -0,0 +1,421 @@
+/**
+ * @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."
+ *
+ * @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);
+// 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(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];
+
+}
+
+
+
+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);
+
+}
+
+
+
+
diff -r 000000000000 -r f5a5a0e5fefb ADXL345_I2C.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ADXL345_I2C.h Mon Apr 23 14:59:48 2012 +0000
@@ -0,0 +1,575 @@
+/**
+ * @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."
+ *
+ * @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(int* readings);
+
+ /**
+ * 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 */
diff -r 000000000000 -r f5a5a0e5fefb HMC5883L.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5883L.cpp Mon Apr 23 14:59:48 2012 +0000
@@ -0,0 +1,174 @@
+/**
+ * @author Jose R. Padron
+ * @author Used HMC6352 library developed by Aaron Berk as template
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * 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
+ *
+ * Honeywell HMC5883Ldigital compass.
+ *
+ * Datasheet:
+ *
+ * http://www.ssec.honeywell.com/magnetic/datasheets/HMC5883L.pdf
+ */
+
+/**
+ * Includes
+ */
+#include "HMC5883L.h"
+
+HMC5883L::HMC5883L(PinName sda, PinName scl) {
+
+ i2c_ = new I2C(sda, scl);
+ //100KHz, as specified by the datasheet.
+ i2c_->frequency(100000);
+
+
+}
+
+
+void HMC5883L::write(int address, int data) {
+
+ char tx[2];
+
+ tx[0]=address;
+ tx[1]=data;
+
+ i2c_->write(HMC5883L_I2C_WRITE,tx,2);
+
+ wait_ms(100);
+
+}
+
+
+void HMC5883L::setSleepMode() {
+
+ write(HMC5883L_MODE, HMC5883L_SLEEP);
+}
+
+void HMC5883L::setDefault(void) {
+
+ write(HMC5883L_CONFIG_A,HMC5883L_10HZ_NORMAL);
+ write(HMC5883L_CONFIG_B,HMC5883L_1_0GA);
+ write(HMC5883L_MODE,HMC5883L_CONTINUOUS);
+ wait_ms(100);
+}
+
+
+void HMC5883L::getAddress(char *buffer) {
+
+ char rx[3];
+ char tx[1];
+ tx[0]=HMC5883L_IDENT_A;
+
+
+ i2c_->write(HMC5883L_I2C_WRITE, tx,1);
+
+ wait_ms(1);
+
+ i2c_->read(HMC5883L_I2C_READ,rx,3);
+
+ buffer[0]=rx[0];
+ buffer[1]=rx[1];
+ buffer[2]=rx[2];
+}
+
+
+
+void HMC5883L::setOpMode(int mode, int ConfigA, int ConfigB) {
+
+
+ write(HMC5883L_CONFIG_A,ConfigA);
+ write(HMC5883L_CONFIG_B,ConfigB);
+ write(HMC5883L_MODE,mode);
+
+
+}
+
+
+
+
+void HMC5883L::readData(int* getMag) {
+
+
+ char tx[1];
+ char rx[2];
+
+
+ tx[0]=HMC5883L_X_MSB;
+ i2c_->write(HMC5883L_I2C_READ,tx,1);
+ i2c_->read(HMC5883L_I2C_READ,rx,2);
+ getMag[0]= (int)rx[0]<<8|(int)rx[1];
+
+
+ tx[0]=HMC5883L_Y_MSB;
+ i2c_->write(HMC5883L_I2C_READ,tx,1);
+ i2c_->read(HMC5883L_I2C_READ,rx,2);
+ getMag[1]= (int)rx[0]<<8|(int)rx[1];
+
+ tx[0]=HMC5883L_Z_MSB;
+ i2c_->write(HMC5883L_I2C_READ,tx,1);
+ i2c_->read(HMC5883L_I2C_READ,rx,2);
+ getMag[2]= (int)rx[0]<<8|(int)rx[1];
+
+}
+
+int HMC5883L::getMx() {
+
+ char tx[1];
+ char rx[2];
+
+
+ tx[0]=HMC5883L_X_MSB;
+ i2c_->write(HMC5883L_I2C_READ,tx,1);
+ i2c_->read(HMC5883L_I2C_READ,rx,2);
+ return ((int)rx[0]<<8|(int)rx[1]);
+
+}
+
+int HMC5883L::getMy() {
+
+ char tx[1];
+ char rx[2];
+
+
+ tx[0]=HMC5883L_Y_MSB;
+ i2c_->write(HMC5883L_I2C_READ,tx,1);
+ i2c_->read(HMC5883L_I2C_READ,rx,2);
+ return ((int)rx[0]<<8|(int)rx[1]);
+
+}
+
+
+int HMC5883L::getMz(){
+
+ char tx[1];
+ char rx[2];
+
+
+ tx[0]=HMC5883L_Z_MSB;
+ i2c_->write(HMC5883L_I2C_READ,tx,1);
+ i2c_->read(HMC5883L_I2C_READ,rx,2);
+ return ((int)rx[0]<<8|(int)rx[1]);
+
+}
\ No newline at end of file
diff -r 000000000000 -r f5a5a0e5fefb HMC5883L.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5883L.h Mon Apr 23 14:59:48 2012 +0000
@@ -0,0 +1,219 @@
+/**
+ * @author Uwe Gartmann
+ * @author Used HMC5883L library developed by Jose R. Padron and Aaron Berk as template
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * 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
+ *
+ * Honeywell HMC5883L digital compass.
+ *
+ * Datasheet:
+ *
+ * http://www.ssec.honeywell.com/magnetic/datasheets/HMC5883L.pdf
+ */
+
+#ifndef HMC5883L_H
+#define HMC5883L_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define HMC5883L_I2C_ADDRESS 0x1E //7-bit address. 0x3C write, 0x3D read.
+#define HMC5883L_I2C_WRITE 0x3C
+#define HMC5883L_I2C_READ 0x3D
+
+//Values Config A
+#define HMC5883L_0_5HZ_NORMAL 0x00
+#define HMC5883L_0_5HZ_POSITIVE 0x01
+#define HMC5883L_0_5HZ_NEGATIVE 0x02
+
+#define HMC5883L_1HZ_NORMAL 0x04
+#define HMC5883L_1HZ_POSITIVE 0x05
+#define HMC5883L_1HZ_NEGATIVE 0x06
+
+#define HMC5883L_2HZ_NORMAL 0x08
+#define HMC5883L_2HZ_POSITIVE 0x09
+#define HMC5883L_2HZ_NEGATIVE 0x0A
+
+#define HMC5883L_5HZ_NORMAL 0x0C
+#define HMC5883L_5HZ_POSITIVE 0x0D
+#define HMC5883L_5HZ_NEGATIVE 0x0E
+
+#define HMC5883L_10HZ_NORMAL 0x10
+#define HMC5883L_10HZ_POSITIVE 0x11
+#define HMC5883L_10HZ_NEGATIVE 0x12
+
+#define HMC5883L_20HZ_NORMAL 0x14
+#define HMC5883L_20HZ_POSITIVE 0x15
+#define HMC5883L_20HZ_NEGATIVE 0x16
+
+#define HMC5883L_50HZ_NORMAL 0x18
+#define HMC5883L_50HZ_POSITIVE 0x19
+#define HMC5883L_50HZ_NEGATIVE 0x1A
+
+//Values Config B
+#define HMC5883L_0_7GA 0x00
+#define HMC5883L_1_0GA 0x20
+#define HMC5883L_1_5GA 0x40
+#define HMC5883L_2_0GA 0x60
+#define HMC5883L_3_2GA 0x80
+#define HMC5883L_3_8GA 0xA0
+#define HMC5883L_4_5GA 0xC0
+#define HMC5883L_6_5GA 0xE0
+
+//Values MODE
+#define HMC5883L_CONTINUOUS 0x00
+#define HMC5883L_SINGLE 0x01
+#define HMC5883L_IDLE 0x02
+#define HMC5883L_SLEEP 0x03
+
+
+
+#define HMC5883L_CONFIG_A 0x00
+#define HMC5883L_CONFIG_B 0x01
+#define HMC5883L_MODE 0x02
+#define HMC5883L_X_MSB 0x03
+#define HMC5883L_X_LSB 0x04
+#define HMC5883L_Z_MSB 0x05
+#define HMC5883L_Z_LSB 0x06
+#define HMC5883L_Y_MSB 0x07
+#define HMC5883L_Y_LSB 0x08
+#define HMC5883L_STATUS 0x09
+#define HMC5883L_IDENT_A 0x0A
+#define HMC5883L_IDENT_B 0x0B
+#define HMC5883L_IDENT_C 0x0C
+
+
+
+/**
+ * Honeywell HMC5883L digital compass.
+ */
+class HMC5883L {
+
+public:
+
+ /**
+ * Constructor.
+ *
+ * @param sda mbed pin to use for SDA line of I2C interface.
+ * @param scl mbed pin to use for SCL line of I2C interface.
+ */
+ HMC5883L(PinName sda, PinName scl);
+
+
+ /**
+ * Enter into sleep mode.
+ *
+ */
+ void setSleepMode();
+
+
+ /**
+ * Set Device in Default Mode.
+ * HMC5883L_CONTINUOUS, HMC5883L_10HZ_NORMAL HMC5883L_1_0GA
+ */
+ void setDefault();
+
+
+ /**
+ * Read the memory location on the device which contains the address.
+ *
+ * @param Pointer to a buffer to hold the address value
+ * Expected H, 4 and 3.
+ */
+ void getAddress(char * address);
+
+
+
+ /**
+ * Set the operation mode.
+ *
+ * @param mode 0x00 -> Continuous
+ * 0x01 -> Single
+ * 0x02 -> Idle
+ * @param ConfigA values
+ * @param ConfigB values
+ */
+ void setOpMode(int mode, int ConfigA, int ConfigB);
+
+ /**
+ * Write to on the device.
+ *
+ * @param address Address to write to.
+ * @param data Data to write.
+ */
+
+ void write(int address, int data);
+
+ /**
+ * Get the output of all three axes.
+ *
+ * @param Pointer to a buffer to hold the magnetics value for the
+ * x-axis, y-axis and z-axis [in that order].
+ */
+ void readData(int* getMag);
+
+ /**
+ * Get the output of X axis.
+ *
+ * @return x-axis magnetic value
+ */
+ int getMx();
+
+ /**
+ * Get the output of Y axis.
+ *
+ * @return y-axis magnetic value
+ */
+ int getMy();
+
+ /**
+ * Get the output of Z axis.
+ *
+ * @return z-axis magnetic value
+ */
+ int getMz();
+
+
+ /**
+ * Get the current operation mode.
+ *
+ * @return Status register values
+ */
+ int getStatus(void);
+
+
+
+ I2C* i2c_;
+
+
+
+};
+
+#endif /* HMC5883L_H */
\ No newline at end of file
diff -r 000000000000 -r f5a5a0e5fefb ITG3200.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200.cpp Mon Apr 23 14:59:48 2012 +0000
@@ -0,0 +1,280 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * 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
+ *
+ * ITG-3200 triple axis, digital interface, gyroscope.
+ *
+ * Datasheet:
+ *
+ * http://invensense.com/mems/gyro/documents/PS-ITG-3200-00-01.4.pdf
+ */
+
+/**
+ * Includes
+ */
+#include "ITG3200.h"
+
+ITG3200::ITG3200(PinName sda, PinName scl) : i2c_(sda, scl) {
+
+ //400kHz, fast mode.
+ i2c_.frequency(400000);
+
+ //Set FS_SEL to 0x03 for proper operation.
+ //See datasheet for details.
+ char tx[2];
+ tx[0] = DLPF_FS_REG;
+ //FS_SEL bits sit in bits 4 and 3 of DLPF_FS register.
+ tx[1] = 0x03 << 3;
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+char ITG3200::getWhoAmI(void){
+
+ //WhoAmI Register address.
+ char tx = WHO_AM_I_REG;
+ char rx;
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+
+ i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+
+ return rx;
+
+}
+
+void ITG3200::setWhoAmI(char address){
+
+ char tx[2];
+ tx[0] = WHO_AM_I_REG;
+ tx[1] = address;
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+char ITG3200::getSampleRateDivider(void){
+
+ char tx = SMPLRT_DIV_REG;
+ char rx;
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+
+ i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+
+ return rx;
+
+}
+
+void ITG3200::setSampleRateDivider(char divider){
+
+ char tx[2];
+ tx[0] = SMPLRT_DIV_REG;
+ tx[1] = divider;
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+int ITG3200::getInternalSampleRate(void){
+
+ char tx = DLPF_FS_REG;
+ char rx;
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+
+ i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+
+ //DLPF_CFG == 0 -> sample rate = 8kHz.
+ if(rx == 0){
+ return 8;
+ }
+ //DLPF_CFG = 1..7 -> sample rate = 1kHz.
+ else if(rx >= 1 && rx <= 7){
+ return 1;
+ }
+ //DLPF_CFG = anything else -> something's wrong!
+ else{
+ return -1;
+ }
+
+}
+
+void ITG3200::setLpBandwidth(char bandwidth){
+
+ char tx[2];
+ tx[0] = DLPF_FS_REG;
+ //Bits 4,3 are required to be 0x03 for proper operation.
+ tx[1] = bandwidth | (0x03 << 3);
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+char ITG3200::getInterruptConfiguration(void){
+
+ char tx = INT_CFG_REG;
+ char rx;
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+
+ i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+
+ return rx;
+
+}
+
+void ITG3200::setInterruptConfiguration(char config){
+
+ char tx[2];
+ tx[0] = INT_CFG_REG;
+ tx[1] = config;
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
+
+bool ITG3200::isPllReady(void){
+
+ char tx = INT_STATUS;
+ char rx;
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+
+ i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+
+ //ITG_RDY bit is bit 4 of INT_STATUS register.
+ if(rx & 0x04){
+ return true;
+ }
+ else{
+ return false;
+ }
+
+}
+
+bool ITG3200::isRawDataReady(void){
+
+ char tx = INT_STATUS;
+ char rx;
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+
+ i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+
+ //RAW_DATA_RDY bit is bit 1 of INT_STATUS register.
+ if(rx & 0x01){
+ return true;
+ }
+ else{
+ return false;
+ }
+
+}
+
+float ITG3200::getTemperature(void){
+
+ char tx = TEMP_OUT_H_REG;
+ char rx[2];
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+
+ i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
+
+ int16_t temperature = ((int) rx[0] << 8) | ((int) rx[1]);
+ //Offset = -35 degrees, 13200 counts. 280 counts/degrees C.
+ return 35.0 + ((temperature + 13200)/280.0);
+
+}
+
+int ITG3200::getGyroX(void){
+
+ char tx = GYRO_XOUT_H_REG;
+ char rx[2];
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+
+ i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
+
+ int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+
+ return output;
+
+}
+
+int ITG3200::getGyroY(void){
+
+ char tx = GYRO_YOUT_H_REG;
+ char rx[2];
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+
+ i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
+
+ int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+
+ return output;
+
+}
+
+int ITG3200::getGyroZ(void){
+
+ char tx = GYRO_ZOUT_H_REG;
+ char rx[2];
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+
+ i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, rx, 2);
+
+ int16_t output = ((int) rx[0] << 8) | ((int) rx[1]);
+
+ return output;
+
+}
+
+char ITG3200::getPowerManagement(void){
+
+ char tx = PWR_MGM_REG;
+ char rx;
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, &tx, 1);
+
+ i2c_.read((ITG3200_I2C_ADDRESS << 1) | 0x01, &rx, 1);
+
+ return rx;
+
+}
+
+void ITG3200::setPowerManagement(char config){
+
+ char tx[2];
+ tx[0] = PWR_MGM_REG;
+ tx[1] = config;
+
+ i2c_.write((ITG3200_I2C_ADDRESS << 1) & 0xFE, tx, 2);
+
+}
diff -r 000000000000 -r f5a5a0e5fefb ITG3200.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ITG3200.h Mon Apr 23 14:59:48 2012 +0000
@@ -0,0 +1,339 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * 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
+ *
+ * ITG-3200 triple axis, digital interface, gyroscope.
+ *
+ * Datasheet:
+ *
+ * http://invensense.com/mems/gyro/documents/PS-ITG-3200-00-01.4.pdf
+ */
+
+#ifndef ITG3200_H
+#define ITG3200_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define ITG3200_I2C_ADDRESS 0x68 //7-bit address.
+
+//-----------
+// Registers
+//-----------
+#define WHO_AM_I_REG 0x00
+#define SMPLRT_DIV_REG 0x15
+#define DLPF_FS_REG 0x16
+#define INT_CFG_REG 0x17
+#define INT_STATUS 0x1A
+#define TEMP_OUT_H_REG 0x1B
+#define TEMP_OUT_L_REG 0x1C
+#define GYRO_XOUT_H_REG 0x1D
+#define GYRO_XOUT_L_REG 0x1E
+#define GYRO_YOUT_H_REG 0x1F
+#define GYRO_YOUT_L_REG 0x20
+#define GYRO_ZOUT_H_REG 0x21
+#define GYRO_ZOUT_L_REG 0x22
+#define PWR_MGM_REG 0x3E
+
+//----------------------------
+// Low Pass Filter Bandwidths
+//----------------------------
+#define LPFBW_256HZ 0x00
+#define LPFBW_188HZ 0x01
+#define LPFBW_98HZ 0x02
+#define LPFBW_42HZ 0x03
+#define LPFBW_20HZ 0x04
+#define LPFBW_10HZ 0x05
+#define LPFBW_5HZ 0x06
+
+/**
+ * ITG-3200 triple axis digital gyroscope.
+ */
+class ITG3200 {
+
+public:
+
+ /**
+ * Constructor.
+ *
+ * Sets FS_SEL to 0x03 for proper opertaion.
+ *
+ * @param sda - mbed pin to use for the SDA I2C line.
+ * @param scl - mbed pin to use for the SCL I2C line.
+ */
+ ITG3200(PinName sda, PinName scl);
+
+ /**
+ * Get the identity of the device.
+ *
+ * @return The contents of the Who Am I register which contains the I2C
+ * address of the device.
+ */
+ char getWhoAmI(void);
+
+ /**
+ * Set the address of the device.
+ *
+ * @param address The I2C slave address to write to the Who Am I register
+ * on the device.
+ */
+ void setWhoAmI(char address);
+
+ /**
+ * Get the sample rate divider.
+ *
+ * @return The sample rate divider as a number from 0-255.
+ */
+ char getSampleRateDivider(void);
+
+ /**
+ * Set the sample rate divider.
+ *
+ * Fsample = Finternal / (divider + 1), where Finternal = 1kHz or 8kHz,
+ * as decidied by the DLPF_FS register.
+ *
+ * @param The sample rate divider as a number from 0-255.
+ */
+ void setSampleRateDivider(char divider);
+
+ /**
+ * Get the internal sample rate.
+ *
+ * @return The internal sample rate in kHz - either 1 or 8.
+ */
+ int getInternalSampleRate(void);
+
+ /**
+ * Set the low pass filter bandwidth.
+ *
+ * Also used to set the internal sample rate.
+ * Pass the #define bandwidth codes as a parameter.
+ *
+ * 256Hz -> 8kHz internal sample rate.
+ * Everything else -> 1kHz internal rate.
+ *
+ * @param bandwidth Low pass filter bandwidth code
+ */
+ void setLpBandwidth(char bandwidth);
+
+ /**
+ * Get the interrupt configuration.
+ *
+ * See datasheet for register contents details.
+ *
+ * 7 6 5 4
+ * +------+------+--------------+------------------+
+ * | ACTL | OPEN | LATCH_INT_EN | INT_ANYRD_2CLEAR |
+ * +------+------+--------------+------------------+
+ *
+ * 3 2 1 0
+ * +---+------------+------------+---+
+ * | 0 | ITG_RDY_EN | RAW_RDY_EN | 0 |
+ * +---+------------+------------+---+
+ *
+ * ACTL Logic level for INT output pin; 1 = active low, 0 = active high.
+ * OPEN Drive type for INT output pin; 1 = open drain, 0 = push-pull.
+ * LATCH_INT_EN Latch mode; 1 = latch until interrupt is cleared,
+ * 0 = 50us pulse.
+ * INT_ANYRD_2CLEAR Latch clear method; 1 = any register read,
+ * 0 = status register read only.
+ * ITG_RDY_EN Enable interrupt when device is ready,
+ * (PLL ready after changing clock source).
+ * RAW_RDY_EN Enable interrupt when data is available.
+ * 0 Bits 1 and 3 of the INT_CFG register should be zero.
+ *
+ * @return the contents of the INT_CFG register.
+ */
+ char getInterruptConfiguration(void);
+
+ /**
+ * Set the interrupt configuration.
+ *
+ * See datasheet for configuration byte details.
+ *
+ * 7 6 5 4
+ * +------+------+--------------+------------------+
+ * | ACTL | OPEN | LATCH_INT_EN | INT_ANYRD_2CLEAR |
+ * +------+------+--------------+------------------+
+ *
+ * 3 2 1 0
+ * +---+------------+------------+---+
+ * | 0 | ITG_RDY_EN | RAW_RDY_EN | 0 |
+ * +---+------------+------------+---+
+ *
+ * ACTL Logic level for INT output pin; 1 = active low, 0 = active high.
+ * OPEN Drive type for INT output pin; 1 = open drain, 0 = push-pull.
+ * LATCH_INT_EN Latch mode; 1 = latch until interrupt is cleared,
+ * 0 = 50us pulse.
+ * INT_ANYRD_2CLEAR Latch clear method; 1 = any register read,
+ * 0 = status register read only.
+ * ITG_RDY_EN Enable interrupt when device is ready,
+ * (PLL ready after changing clock source).
+ * RAW_RDY_EN Enable interrupt when data is available.
+ * 0 Bits 1 and 3 of the INT_CFG register should be zero.
+ *
+ * @param config Configuration byte to write to INT_CFG register.
+ */
+ void setInterruptConfiguration(char config);
+
+ /**
+ * Check the ITG_RDY bit of the INT_STATUS register.
+ *
+ * @return True if the ITG_RDY bit is set, corresponding to PLL ready,
+ * false if the ITG_RDY bit is not set, corresponding to PLL not
+ * ready.
+ */
+ bool isPllReady(void);
+
+ /**
+ * Check the RAW_DATA_RDY bit of the INT_STATUS register.
+ *
+ * @return True if the RAW_DATA_RDY bit is set, corresponding to new data
+ * in the sensor registers, false if the RAW_DATA_RDY bit is not
+ * set, corresponding to no new data yet in the sensor registers.
+ */
+ bool isRawDataReady(void);
+
+ /**
+ * Get the temperature of the device.
+ *
+ * @return The temperature in degrees celsius.
+ */
+ float getTemperature(void);
+
+ /**
+ * Get the output for the x-axis gyroscope.
+ *
+ * Typical sensitivity is 14.375 LSB/(degrees/sec).
+ *
+ * @return The output on the x-axis in raw ADC counts.
+ */
+ int getGyroX(void);
+
+ /**
+ * Get the output for the y-axis gyroscope.
+ *
+ * Typical sensitivity is 14.375 LSB/(degrees/sec).
+ *
+ * @return The output on the y-axis in raw ADC counts.
+ */
+ int getGyroY(void);
+
+ /**
+ * Get the output on the z-axis gyroscope.
+ *
+ * Typical sensitivity is 14.375 LSB/(degrees/sec).
+ *
+ * @return The output on the z-axis in raw ADC counts.
+ */
+ int getGyroZ(void);
+
+ /**
+ * Get the power management configuration.
+ *
+ * See the datasheet for register contents details.
+ *
+ * 7 6 5 4
+ * +---------+-------+---------+---------+
+ * | H_RESET | SLEEP | STBY_XG | STBY_YG |
+ * +---------+-------+---------+---------+
+ *
+ * 3 2 1 0
+ * +---------+----------+----------+----------+
+ * | STBY_ZG | CLK_SEL2 | CLK_SEL1 | CLK_SEL0 |
+ * +---------+----------+----------+----------+
+ *
+ * H_RESET Reset device and internal registers to the power-up-default settings.
+ * SLEEP Enable low power sleep mode.
+ * STBY_XG Put gyro X in standby mode (1=standby, 0=normal).
+ * STBY_YG Put gyro Y in standby mode (1=standby, 0=normal).
+ * STBY_ZG Put gyro Z in standby mode (1=standby, 0=normal).
+ * CLK_SEL Select device clock source:
+ *
+ * CLK_SEL | Clock Source
+ * --------+--------------
+ * 0 Internal oscillator
+ * 1 PLL with X Gyro reference
+ * 2 PLL with Y Gyro reference
+ * 3 PLL with Z Gyro reference
+ * 4 PLL with external 32.768kHz reference
+ * 5 PLL with external 19.2MHz reference
+ * 6 Reserved
+ * 7 Reserved
+ *
+ * @return The contents of the PWR_MGM register.
+ */
+ char getPowerManagement(void);
+
+ /**
+ * Set power management configuration.
+ *
+ * See the datasheet for configuration byte details
+ *
+ * 7 6 5 4
+ * +---------+-------+---------+---------+
+ * | H_RESET | SLEEP | STBY_XG | STBY_YG |
+ * +---------+-------+---------+---------+
+ *
+ * 3 2 1 0
+ * +---------+----------+----------+----------+
+ * | STBY_ZG | CLK_SEL2 | CLK_SEL1 | CLK_SEL0 |
+ * +---------+----------+----------+----------+
+ *
+ * H_RESET Reset device and internal registers to the power-up-default settings.
+ * SLEEP Enable low power sleep mode.
+ * STBY_XG Put gyro X in standby mode (1=standby, 0=normal).
+ * STBY_YG Put gyro Y in standby mode (1=standby, 0=normal).
+ * STBY_ZG Put gyro Z in standby mode (1=standby, 0=normal).
+ * CLK_SEL Select device clock source:
+ *
+ * CLK_SEL | Clock Source
+ * --------+--------------
+ * 0 Internal oscillator
+ * 1 PLL with X Gyro reference
+ * 2 PLL with Y Gyro reference
+ * 3 PLL with Z Gyro reference
+ * 4 PLL with external 32.768kHz reference
+ * 5 PLL with external 19.2MHz reference
+ * 6 Reserved
+ * 7 Reserved
+ *
+ * @param config The configuration byte to write to the PWR_MGM register.
+ */
+ void setPowerManagement(char config);
+
+private:
+
+ I2C i2c_;
+
+};
+
+#endif /* ITG3200_H */
diff -r 000000000000 -r f5a5a0e5fefb TextLCD.lib
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib Mon Apr 23 14:59:48 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/TextLCD/#44f34c09bd37
diff -r 000000000000 -r f5a5a0e5fefb main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Apr 23 14:59:48 2012 +0000
@@ -0,0 +1,474 @@
+// IMU for Sparkfun 9DOF Sensor Stick
+
+#include "mbed.h"
+#include "ADXL345_I2C.h"
+#include "ITG3200.h"
+#include "HMC5883L.h"
+#include "TextLCD.h"
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+ADXL345_I2C accelerometer(p9, p10);
+ITG3200 gyro(p9, p10);
+HMC5883L compass(p9, p10);
+TextLCD lcd(p24, p26, p27, p28, p29, p30);
+Serial pc(USBTX, USBRX);
+
+#define pi 3.14159265
+
+#define N 3
+#define N_LWMA 10
+
+double* RK4( double, double[N], double[N] );
+double* func( double[N], double[N] );
+double* LWMA( double[N] );
+double* EKF_predict( double[N], double[N] );
+double* EKF_correct( double[N], double[N], double[N] );
+
+// 0 1 2
+// [ accXn-1 accXn-2 ... ] 0
+// zBuf = [ accYn-1 accYn-2 ... ] 1
+// [ accZn-1 accZn-2 ... ] 2
+double z_buf[N][N_LWMA] = {0}; // For LWMA
+
+double P[N][N] = {{1,0,0},{0,1,0},{0,0,1}}; // For EKF
+
+int main(){
+
+ pc.baud(921600);
+
+ // loop
+ int correct_loop = 0;
+ int calib_loop = 1000;
+ int lcd_loop = 0;
+
+ //double dt_wait = 0.00514;
+ double dt_wait = 0.0046;
+ double dt = 0.01;
+ double t = 0;
+
+ int bit_acc[N] = {0}; // Buffer of the accelerometer
+ int get_mag[N] = {0}; // Buffer of the compass
+
+ // Calibration routine
+ double calib_acc[N] = {0};
+ double calib_gyro_buf[N] = {0};
+ double calib_gyro[N] = {0};
+ double compass_basis_buf[N] = {0};
+ double compass_basis_rad = 0;
+
+ // Getting data
+ double acc[N] = {0};
+ double gyro_deg[N] = {0};
+ double gyro_rad[N] = {0};
+ int mag[N] = {0};
+
+ // Measurement algorithm
+ double ang_acc[2] = {0};
+ double ang_deg[N] = {0};
+ double ang_rad[N] = {0};
+ double zLWMA[N] = {0};
+ double compass_rad = 0;
+ double compass_deg = 0;
+
+ for( int i=0;i<N_LWMA;i++ ){
+ z_buf[2][i] = 1;
+ }
+
+ double* p_ang_RK4;
+ double* p_ang_EKF;
+ double* p_zLWMA_buf;
+
+ // *** Setting up accelerometer ***
+ // These are here to test whether any of the initialization fails. It will print the failure.
+ if(accelerometer.setPowerControl(0x00)){
+ pc.printf("didn't intitialize power control\n\r");
+ return 0;
+ }
+ // Full resolution, +/-16g, 4mg/LSB.
+ wait(.001);
+
+ if(accelerometer.setDataFormatControl(0x0B)){
+ pc.printf("didn't set data format\n\r");
+ return 0; }
+ wait(.001);
+
+ // 3.2kHz data rate.
+ if(accelerometer.setDataRate(ADXL345_3200HZ)){
+ pc.printf("didn't set data rate\n\r");
+ return 0;
+ }
+ wait(.001);
+
+ if(accelerometer.setPowerControl(MeasurementMode)) {
+ pc.printf("didn't set the power control to measurement\n\r");
+ return 0;
+ }
+ // *** Setting up accelerometer ***
+
+ // *** Setting up gyro ***
+ gyro.setLpBandwidth(LPFBW_42HZ);
+
+ // *** Setting up compass ***
+ compass.setDefault();
+
+ // Wait some time for all sensors (Need at least 5ms)
+ wait(0.1);
+
+ // *** Calibration routine ***
+ pc.printf("\n\rDon't touch! Calibrating now!\n\r");
+ lcd.locate(0, 0); lcd.printf("Don't touch!");
+ lcd.locate(0, 1); lcd.printf("Calibrating now!");
+ led1 = 1;
+ for( int i=0;i<calib_loop;i++ ){
+ accelerometer.getOutput(bit_acc);
+ compass.readData(get_mag);
+ calib_gyro_buf[0] = gyro.getGyroX();
+ calib_gyro_buf[1] = gyro.getGyroY();
+ calib_gyro_buf[2] = gyro.getGyroZ();
+ for( int j=0;j<N;j++ ){
+ calib_acc[j] += (int16_t)bit_acc[j];
+ calib_gyro[j] += calib_gyro_buf[j];
+ compass_basis_buf[j] += (int16_t)get_mag[j];
+ }
+ if( i>calib_loop*3/4 ){
+ led4 = 1;
+ }else if( i>calib_loop/2 ){
+ led3 = 1;
+ }else if( i>calib_loop/4 ){
+ led2 = 1;
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ calib_acc[i] = calib_acc[i]/calib_loop;
+ calib_gyro[i] = calib_gyro[i]/calib_loop;
+ compass_basis_buf[i] = compass_basis_buf[i]/calib_loop;
+ }
+ compass_basis_rad = compass_basis_buf[1]/compass_basis_buf[0];
+ compass_basis_rad = atan(compass_basis_rad);
+ led1 = 0; led2 = 0; led3 = 0; led4 = 0;
+ pc.printf("accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r");
+ pc.printf("%f, %f, %f, %f, %f, %f, %f\n\r",calib_acc[0],calib_acc[1],calib_acc[2],calib_gyro[0],calib_gyro[1],calib_gyro[2],compass_basis_rad*180/pi);
+ lcd.cls();
+ lcd.locate(0, 0); lcd.printf("Starting IMU");
+ lcd.locate(0, 1); lcd.printf("IMU with EKF");
+ for( int i=0;i<3;i++ ){
+ wait(0.5);
+ led1 = 1; led2 = 1; led3 = 1; led4 = 1;
+ wait(0.5);
+ led1 = 0; led2 = 0; led3 = 0; led4 = 0;
+ }
+ // *** Calibration routine ***
+
+ lcd.cls();
+ pc.printf("Starting IMU unit \n\r");
+ pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ MagX MagY MagZ\n\r");
+ while( 1 ){
+ //for( int i=0;i<10000;i++ ){
+
+ // Updating accelerometer and compass
+ accelerometer.getOutput(bit_acc);
+ compass.readData(get_mag);
+
+ // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga])
+ acc[0] = -((int16_t)bit_acc[0]-calib_acc[0])*0.004;
+ acc[1] = ((int16_t)bit_acc[1]-calib_acc[1])*0.004;
+ acc[2] = ((int16_t)bit_acc[2]-calib_acc[2])*0.004+1;
+
+ gyro_deg[0] = (gyro.getGyroX()-calib_gyro[0])/14.375;
+ gyro_deg[1] = (gyro.getGyroY()-calib_gyro[1])/14.375;
+ gyro_deg[2] = (gyro.getGyroZ()-calib_gyro[2])/14.375;
+
+ for( int i=0;i<N;i++ ){ mag[0] = (int16_t)get_mag[0]; }
+
+ // Low pass filter for acc
+ //for( int i=0;i<N;i++ ){ if( -0.05<acc[i] && acc[i]<0.05 ){ acc[i] = 0; } }
+
+ for( int i=0;i<N;i++ ){ if( acc[i]<-2 ){ acc[i] = -2; } }
+ for( int i=0;i<N;i++ ){ if( acc[i]>2 ){ acc[i] = 2; } }
+
+ // Low pass filter for gyro
+ //for( int i=0;i<N;i++ ){ if( -0.5<gyro_deg[i] && gyro_deg[i]<0.5 ){ gyro_deg[i] = 0; } }
+
+ for( int i=0;i<N;i++ ){ gyro_rad[i] = gyro_deg[i]*pi/180; }
+
+ // Compass yaw
+ /*compass_rad = (double)mag[1]/mag[0];
+ compass_rad = atan(compass_rad);
+ //compass_rad = compass_rad-compass_basis_rad;
+ compass_deg = compass_rad*180/pi;
+ */
+ // LWMA (Observation)
+ p_zLWMA_buf = LWMA(acc);
+ for( int i=0;i<N;i++ ){ zLWMA[i] = *p_zLWMA_buf; p_zLWMA_buf = p_zLWMA_buf+1; }
+ // LWMA angle
+ ang_acc[0] = asin(zLWMA[1])*180/pi;
+ ang_acc[1] = asin(zLWMA[0])*180/pi;
+
+ // RK4 (Prediction)
+ p_ang_RK4 = RK4(dt,ang_rad,gyro_rad);
+ for( int i=0;i<N;i++ ){ ang_rad[i] = *p_ang_RK4; p_ang_RK4 = p_ang_RK4+1; }
+
+ // EKF (Correction)
+ EKF_predict(ang_rad,gyro_rad);
+ correct_loop++;
+ if ( correct_loop>=10 ){
+ p_ang_EKF = EKF_correct(ang_rad,gyro_rad,zLWMA);
+ for ( int i=0;i<N;i++ ){ ang_deg[i] = *p_ang_EKF; p_ang_EKF = p_ang_EKF+1; }
+ correct_loop = 0;
+ }
+ for( int i=0;i<N;i++ ){ ang_deg[i] = 1.76*ang_rad[i]*180/pi; }
+
+ //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], mag[0], mag[1], mag[2]);
+ //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2]);
+ //pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, gyro_deg[0], gyro_deg[1], gyro_deg[2], ang_deg[0], ang_deg[1], ang_deg[2]);
+ //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f\n\r", t, acc[0], acc[1], acc[2], zLWMA[0], zLWMA[1], zLWMA[2]);
+ //pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f\n\r", t, zLWMA[0], zLWMA[1], zLWMA[2], ang_acc[0], ang_acc[1]);
+ //pc.printf("%d, %d, %f\n\r",mag[0], mag[1], compass_deg);
+ //pc.printf("%7.2f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, ang_acc[0], ang_acc[1], ang_deg[0], ang_deg[1], ang_deg[2]);
+ pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, gyro_deg[0], gyro_deg[1], gyro_deg[2], ang_deg[0], ang_deg[1], ang_deg[2]);
+ //pc.printf("%7.2f, %7.1f, %7.1f, 0.0\n\r", t, ang_deg[0], ang_deg[1]);
+ lcd_loop++;
+ if( lcd_loop>=20 ){
+ lcd.locate(0, 0); lcd.printf("phi:%6.1f [deg]", ang_deg[0]);
+ lcd.locate(0, 1); lcd.printf("the:%6.1f [deg]", ang_deg[1]);
+ lcd_loop = 0;
+ }
+
+ wait(dt_wait);
+ t += dt;
+
+ }
+
+ led1 = 1; led2 = 1; led3 = 1; led4 = 1;
+
+}
+
+
+double* EKF_predict( double y[N], double x[N] ){
+ // x = F * x;
+ // P = F * P * F' + G * Q * G';
+
+ //double Q[N][N] = { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1} };
+ double Q[N][N] = { {0.00263, 0, 0}, {0, 0.00373, 0}, {0, 0, 0.00509} };
+
+ double Fjac[N][N] = {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0}, {-x[1]*sin(y[0])-x[2]*cos(y[0]), 0, 0}, {x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1]), x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1])), 0}};
+ double Fjac_t[N][N] = {{x[1]*cos(y[0])*tan(y[1])-x[2]*sin(y[0])*tan(y[1]), -x[1]*sin(y[0])-x[2]*cos(y[0]), x[1]*cos(y[0])/cos(y[1])-x[2]*sin(y[0])/cos(y[1])}, {x[1]*sin(y[0])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])/(cos(y[1])*cos(y[1])), 0, x[1]*sin(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))+x[2]*cos(y[0])*sin(y[1])/(cos(y[1])*cos(y[1]))}, {0, 0, 0}};
+ double Gjac[N][N] = {{1, sin(y[0])*tan(y[1]), cos(y[0])*tan(y[1])}, {0, cos(y[0]), -sin(y[0])}, {0, sin(y[0])/cos(y[1]), cos(y[0])/cos(y[1])}};
+ double Gjac_t[N][N] = {{1, 0, 0}, {sin(y[0])*tan(y[1]), cos(y[0]), sin(y[0])/cos(y[1])}, {cos(y[0])*tan(y[1]), -sin(y[0]), cos(y[0])/cos(y[1])}};
+
+ double FPF[N][N] = {0};
+ double GQG[N][N] = {0};
+
+ double FP[N][N] = {0};
+ double GQ[N][N] = {0};
+
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ for( int k=0;k<N;k++ ){
+ FP[i][j] += Fjac[i][k]*P[k][j];
+ GQ[i][j] += Gjac[i][k]*Q[k][j];
+
+ }
+ }
+ }
+
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ for( int k=0;k<N;k++ ){
+ FPF[i][j] += FP[i][k]*Fjac_t[k][j];
+ GQG[i][j] += GQ[i][k]*Gjac_t[k][j];
+ }
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ P[i][j] = FPF[i][j]+GQG[i][j];
+ }
+ }
+
+ return 0;
+
+}
+
+double* EKF_correct( double y[N], double x[N], double z[N] ){
+ // K = P * H' / ( H * P * H' + R )
+ // x = x + K * ( yobs(t) - H * x )
+ // P = P - K * H * P
+
+ //double R[N][N] = { {0.15, 0, 0}, {0, 0.15, 0}, {0, 0, 0.15} };
+ double R[N][N] = { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} };
+
+ double Hjac[N][N] = {{0, cos(y[1]), 0}, {cos(y[0]), 0, 0}, {-sin(y[0])*cos(y[1]), -cos(y[0])*sin(y[1]), 0}};
+ double Hjac_t[N][N] = {{0, cos(y[0]), -sin(y[0])*cos(y[1])}, {cos(y[1]), 0, -cos(y[0])*sin(y[1])}, {0, 0, 0}};
+ double K[N][N] = {0}; // Kalman gain
+ double K_deno[N][N] = {0}; // Denominator of the kalman gain
+ double det_K_deno_inv = 0;
+ double K_deno_inv[N][N] = {0};
+ double HPH[N][N] = {0};
+ double HP[N][N] = {0};
+ double PH[N][N] = {0};
+ double KHP[N][N] = {0};
+
+ double Hx[N] = {0};
+ double z_Hx[N] = {0};
+ double Kz_Hx[N] = {0};
+
+ double* py = y;
+
+ // Kalman gain
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ for( int k=0;k<N;k++ ){
+ HP[i][j] += Hjac[i][k]*P[k][j];
+ PH[i][j] += P[i][k]*Hjac_t[k][j];
+ }
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ for( int k=0;k<N;k++ ){
+ HPH[i][j] += HP[i][k]*Hjac_t[k][j];
+ }
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ K_deno[i][j] = HPH[i][j]+R[i][j];
+ }
+ }
+ // Inverce matrix
+ det_K_deno_inv = K_deno[0][0]*K_deno[1][1]*K_deno[2][2]+K_deno[1][0]*K_deno[2][1]*K_deno[0][2]+K_deno[2][0]*K_deno[0][1]*K_deno[1][2]-K_deno[0][0]*K_deno[2][1]*K_deno[1][2]-K_deno[2][0]*K_deno[1][1]*K_deno[0][2]-K_deno[1][0]*K_deno[0][1]*K_deno[2][2];
+ K_deno_inv[0][0] = (K_deno[1][1]*K_deno[2][2]-K_deno[1][2]*K_deno[2][1])/det_K_deno_inv;
+ K_deno_inv[0][1] = (K_deno[0][2]*K_deno[2][1]-K_deno[0][1]*K_deno[2][2])/det_K_deno_inv;
+ K_deno_inv[0][2] = (K_deno[0][1]*K_deno[1][2]-K_deno[0][2]*K_deno[1][1])/det_K_deno_inv;
+ K_deno_inv[1][0] = (K_deno[1][2]*K_deno[2][0]-K_deno[1][0]*K_deno[2][2])/det_K_deno_inv;
+ K_deno_inv[1][1] = (K_deno[0][0]*K_deno[2][2]-K_deno[0][2]*K_deno[2][0])/det_K_deno_inv;
+ K_deno_inv[1][2] = (K_deno[0][2]*K_deno[1][0]-K_deno[0][0]*K_deno[1][2])/det_K_deno_inv;
+ K_deno_inv[2][0] = (K_deno[1][0]*K_deno[2][1]-K_deno[1][1]*K_deno[2][0])/det_K_deno_inv;
+ K_deno_inv[2][1] = (K_deno[0][1]*K_deno[2][0]-K_deno[0][0]*K_deno[2][1])/det_K_deno_inv;
+ K_deno_inv[2][2] = (K_deno[0][0]*K_deno[1][1]-K_deno[0][1]*K_deno[1][0])/det_K_deno_inv;
+
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ for( int k=0;k<N;k++ ){
+ K[i][j] += PH[i][k]*K_deno_inv[k][j];
+ }
+ }
+ }
+
+ // Filtering
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ Hx[i] += Hjac[i][j]*y[j];
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ z_Hx[i] = z[i]-Hx[i];
+ }
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ Kz_Hx[i] += K[i][j]*z_Hx[j];
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ y[i] = y[i]+Kz_Hx[i];
+ }
+
+ // Covarience
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ for( int k=0;k<N;k++ ){
+ KHP[i][j] += K[i][k]*HP[k][j];
+ }
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ for( int j=0;j<N;j++ ){
+ P[i][j] = P[i][j]-KHP[i][j];
+ }
+ }
+
+ return py;
+
+}
+
+double* LWMA( double z[N] ){
+
+ double zLWMA[N] = {0};
+ double zLWMA_num[N] = {0};
+ double zLWMA_den = 0;
+
+ double* pzLWMA = zLWMA;
+
+ for( int i=1;i<N_LWMA;i++ ){
+ for( int j=0;j<N;j++ ){
+ z_buf[j][N_LWMA-i] = z_buf[j][N_LWMA-i-1];
+ }
+ }
+ for( int i=0;i<N;i++ ){
+ z_buf[i][0] = z[i];
+ }
+
+ for( int i=0;i<N_LWMA;i++ ){
+ for( int j=0;j<N;j++ ){
+ zLWMA_num[j] += (N_LWMA-i)*z_buf[j][i];
+ }
+ zLWMA_den += N_LWMA-i;
+ }
+ for( int i=0;i<N;i++ ){
+ zLWMA[i] = zLWMA_num[i]/zLWMA_den;
+ }
+
+ return pzLWMA;
+
+}
+
+double* RK4( double dt, double y[N], double x[N] ){
+
+ double yBuf[N] = {0};
+ double k[N][4] = {0};
+
+ double* p_y = y;
+
+ double* pk1;
+ double* pk2;
+ double* pk3;
+ double* pk4;
+
+ for( int i=0;i<N;i++){ yBuf[i] = y[i]; }
+ pk1 = func (yBuf,x);
+ for( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; }
+
+ for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][1]; }
+ pk2 = func (yBuf,x);
+ for( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; }
+
+ for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5*dt*k[i][2]; }
+ pk3 = func (yBuf,x);
+ for( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; }
+
+ for( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; }
+ pk4 = func (yBuf,x);
+ for( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; }
+
+ for( int i=0;i<N;i++){ y[i] = y[i]+dt*(k[i][0]+2.0*k[i][1]+2.0*k[i][2]+k[i][3])/6.0; }
+
+ return p_y;
+
+}
+
+double* func( double y[N], double x[N] ){
+
+ double f[N] = {0};
+ double* p_f = f;
+
+ f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]);
+ f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]);
+ f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]);
+
+ return p_f;
+
+}
diff -r 000000000000 -r f5a5a0e5fefb mbed.bld
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld Mon Apr 23 14:59:48 2012 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/b4b9f287a47e
hige dura