hige dura
/
IMU_demo
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