Revision 0:fd289b2e6b74, committed 2014-12-09

Comitter:

jnagendran3

Date:

Tue Dec 09 01:15:37 2014 +0000

Commit message:

first

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/I2Cdev.cpp	Tue Dec 09 01:15:37 2014 +0000
@@ -0,0 +1,276 @@
+// ported from arduino library: https://github.com/jrowberg/i2cdevlib
+// written by szymon gaertig (email: szymon@gaertig.com.pl, website: szymongaertig.pl)
+// Changelog:
+// 2013-01-08 - first release
+
+#include "I2Cdev.h"
+
+I2Cdev::I2Cdev(): i2c(I2C_SDA,I2C_SCL), debugSerial(USBTX, USBRX)
+{
+
+}
+
+I2Cdev::I2Cdev(PinName i2cSda, PinName i2cScl): i2c(i2cSda,i2cScl), debugSerial(USBTX, USBRX)
+{
+      i2c.frequency(100000);
+}
+
+/** Read a single bit from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-7)
+ * @param data Container for single bit value
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) {
+    uint8_t b;
+    uint8_t count = readByte(devAddr, regAddr, &b, timeout);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read a single bit from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-15)
+ * @param data Container for single bit value
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) {
+    uint16_t b;
+    uint8_t count = readWord(devAddr, regAddr, &b, timeout);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read multiple bits from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-7)
+ * @param length Number of bits to read (not more than 8)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) {
+    // 01101001 read byte
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    //    010   masked
+    //   -> 010 shifted
+    uint8_t count, b;
+    if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        b &= mask;
+        b >>= (bitStart - length + 1);
+        *data = b;
+    }
+    return count;
+}
+
+/** Read multiple bits from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-15)
+ * @param length Number of bits to read (not more than 16)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
+ */
+int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) {
+    // 1101011001101001 read byte
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    //    010           masked
+    //           -> 010 shifted
+    uint8_t count;
+    uint16_t w;
+    if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) {
+        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        w &= mask;
+        w >>= (bitStart - length + 1);
+        *data = w;
+    }
+    return count;
+}
+/** Read single byte from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for byte value read from device
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) {
+    
+    return readBytes(devAddr, regAddr, 1, data, timeout);
+}
+
+/** Read single word from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for word value read from device
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) {
+    return readWords(devAddr, regAddr, 1, data, timeout);
+}
+
+/** Read multiple bytes from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of bytes to read
+ * @param data Buffer to store read data in
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Number of bytes read (-1 indicates failure)
+ */
+int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout)
+{
+    char command[1];
+    command[0] = regAddr;
+    char *redData = (char*)malloc(length);
+    i2c.write(devAddr<<1, command, 1, true);
+    i2c.read(devAddr<<1, redData, length);
+    for(int i =0; i < length; i++) {
+        data[i] = redData[i];
+        //debugSerial.printf("We read %x\n" , redData[i]);
+
+    }
+    free (redData);
+    return length;
+}
+
+int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout)
+{
+    return 0;
+}
+
+/** write a single bit in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-7)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
+    uint8_t b;
+    readByte(devAddr, regAddr, &b);
+    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+    return writeByte(devAddr, regAddr, b);
+}
+
+/** write a single bit in a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-15)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) {
+    uint16_t w;
+    readWord(devAddr, regAddr, &w);
+    w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum));
+    return writeWord(devAddr, regAddr, w);
+}
+
+/** Write multiple bits in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-7)
+ * @param length Number of bits to write (not more than 8)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
+    //      010 value to write
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    // 00011100 mask byte
+    // 10101111 original value (sample)
+    // 10100011 original & ~mask
+    // 10101011 masked | value
+    uint8_t b;
+    if (readByte(devAddr, regAddr, &b) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        b &= ~(mask); // zero all important bits in existing byte
+        b |= data; // combine data with existing byte
+        return writeByte(devAddr, regAddr, b);
+    } else {
+        return false;
+    }
+}
+
+/** Write multiple bits in a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-15)
+ * @param length Number of bits to write (not more than 16)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) {
+    //              010 value to write
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    // 0001110000000000 mask byte
+    // 1010111110010110 original value (sample)
+    // 1010001110010110 original & ~mask
+    // 1010101110010110 masked | value
+    uint16_t w;
+    if (readWord(devAddr, regAddr, &w) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        w &= ~(mask); // zero all important bits in existing word
+        w |= data; // combine data with existing word
+        return writeWord(devAddr, regAddr, w);
+    } else {
+        return false;
+    }
+}
+
+/** Write single byte to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New byte value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
+    return writeBytes(devAddr, regAddr, 1, &data);
+}
+
+/** Write single word to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New word value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) {
+    return writeWords(devAddr, regAddr, 1, &data);
+}
+
+bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
+{
+    i2c.start();
+    i2c.write(devAddr<<1);
+    i2c.write(regAddr);
+    for(int i = 0; i < length; i++) {
+        i2c.write(data[i]);
+    }
+    i2c.stop();
+    return true;
+}
+
+bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data)
+{
+    return true;
+}
+
+uint16_t I2Cdev::readTimeout(void)
+{
+    return 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/I2Cdev.h	Tue Dec 09 01:15:37 2014 +0000
@@ -0,0 +1,44 @@
+//ported from arduino library: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050
+//written by szymon gaertig (email: szymon@gaertig.com.pl)
+//
+//Changelog: 
+//2013-01-08 - first beta release
+
+#ifndef I2Cdev_h
+#define I2Cdev_h
+
+#include "mbed.h"
+
+#define I2C_SDA p9
+#define I2C_SCL p10
+
+class I2Cdev {
+    private:
+        I2C i2c;
+        Serial debugSerial;
+    public:
+        I2Cdev();
+        I2Cdev(PinName i2cSda, PinName i2cScl);        
+        
+        int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout());
+        int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout());
+        int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout());
+        int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout());
+        int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout());
+        int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout());
+        int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout());
+        int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout());
+
+        bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+        bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+        bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+        bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+        bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+        bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+        bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+        bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+        static uint16_t readTimeout(void);
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM9DS0.cpp	Tue Dec 09 01:15:37 2014 +0000
@@ -0,0 +1,466 @@
+#include "LSM9DS0.h"
+
+LSM9DS0::LSM9DS0(PinName sda, PinName scl, uint8_t gAddr, uint8_t xmAddr)
+{
+    // xmAddress and gAddress will store the 7-bit I2C address, if using I2C.
+    xmAddress = xmAddr;
+    gAddress = gAddr;
+    
+    i2c_ = new I2Cdev(sda, scl);
+}
+
+uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl, 
+                        gyro_odr gODR, accel_odr aODR, mag_odr mODR)
+{
+    // Store the given scales in class variables. These scale variables
+    // are used throughout to calculate the actual g's, DPS,and Gs's.
+    gScale = gScl;
+    aScale = aScl;
+    mScale = mScl;
+    
+    // Once we have the scale values, we can calculate the resolution
+    // of each sensor. That's what these functions are for. One for each sensor
+    calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
+    calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
+    calcaRes(); // Calculate g / ADC tick, stored in aRes variable
+    
+    
+    // To verify communication, we can read from the WHO_AM_I register of
+    // each device. Store those in a variable so we can return them.
+    uint8_t gTest = gReadByte(WHO_AM_I_G);      // Read the gyro WHO_AM_I
+    uint8_t xmTest = xmReadByte(WHO_AM_I_XM);   // Read the accel/mag WHO_AM_I
+    
+    // Gyro initialization stuff:
+    initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
+    setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
+    setGyroScale(gScale); // Set the gyro range
+    
+    // Accelerometer initialization stuff:
+    initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
+    setAccelODR(aODR); // Set the accel data rate.
+    setAccelScale(aScale); // Set the accel range.
+    
+    // Magnetometer initialization stuff:
+    initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
+    setMagODR(mODR); // Set the magnetometer output data rate.
+    setMagScale(mScale); // Set the magnetometer's range.
+    
+    // Once everything is initialized, return the WHO_AM_I registers we read:
+    return (xmTest << 8) | gTest;
+}
+
+void LSM9DS0::initGyro()
+{
+
+    gWriteByte(CTRL_REG1_G, 0x0F); // Normal mode, enable all axes
+    gWriteByte(CTRL_REG2_G, 0x00); // Normal mode, high cutoff frequency
+    gWriteByte(CTRL_REG3_G, 0x88);  //Interrupt enabled on both INT_G  and I2_DRDY
+    gWriteByte(CTRL_REG4_G, 0x00); // Set scale to 245 dps
+    gWriteByte(CTRL_REG5_G, 0x00); //Init default values
+    
+}
+
+void LSM9DS0::initAccel()
+{
+    xmWriteByte(CTRL_REG0_XM, 0x00);  
+    xmWriteByte(CTRL_REG1_XM, 0x57); // 50Hz data rate, x/y/z all enabled                                                
+    xmWriteByte(CTRL_REG2_XM, 0x00); // Set scale to 2g
+    xmWriteByte(CTRL_REG3_XM, 0x04); // Accelerometer data ready on INT1_XM (0x04)
+
+}
+
+void LSM9DS0::initMag()
+{   
+    xmWriteByte(CTRL_REG5_XM, 0x94); // Mag data rate - 100 Hz, enable temperature sensor
+    xmWriteByte(CTRL_REG6_XM, 0x00); // Mag scale to +/- 2GS
+    xmWriteByte(CTRL_REG7_XM, 0x00); // Continuous conversion mode
+    xmWriteByte(CTRL_REG4_XM, 0x04); // Magnetometer data ready on INT2_XM (0x08)
+    xmWriteByte(INT_CTRL_REG_M, 0x09); // Enable interrupts for mag, active-low, push-pull
+}
+
+void LSM9DS0::calLSM9DS0(float * gbias, float * abias)
+{  
+  uint8_t data[6] = {0, 0, 0, 0, 0, 0};
+  int16_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
+  int samples, ii;
+  
+  // First get gyro bias
+  uint8_t c = gReadByte(CTRL_REG5_G);
+  gWriteByte(CTRL_REG5_G, c | 0x40);         // Enable gyro FIFO  
+  wait_ms(20);                                 // Wait for change to take effect
+  gWriteByte(FIFO_CTRL_REG_G, 0x20 | 0x1F);  // Enable gyro FIFO stream mode and set watermark at 32 samples
+  wait_ms(1000);  // delay 1000 milliseconds to collect FIFO samples
+  
+  samples = (gReadByte(FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples
+
+  for(ii = 0; ii < samples ; ii++) {            // Read the gyro data stored in the FIFO
+
+    data[0] = gReadByte(OUT_X_L_G);
+    data[1] = gReadByte(OUT_X_H_G);
+    data[2] = gReadByte(OUT_Y_L_G);
+    data[3] = gReadByte(OUT_Y_H_G);
+    data[4] = gReadByte(OUT_Z_L_G);
+    data[5] = gReadByte(OUT_Z_H_G);
+  
+    gyro_bias[0] += (((int16_t)data[1] << 8) | data[0]);
+    gyro_bias[1] += (((int16_t)data[3] << 8) | data[2]);
+    gyro_bias[2] += (((int16_t)data[5] << 8) | data[4]);
+  }  
+
+  gyro_bias[0] /= samples; // average the data
+  gyro_bias[1] /= samples; 
+  gyro_bias[2] /= samples; 
+  
+  gbias[0] = (float)gyro_bias[0]*gRes;  // Properly scale the data to get deg/s
+  gbias[1] = (float)gyro_bias[1]*gRes;
+  gbias[2] = (float)gyro_bias[2]*gRes;
+  
+  c = gReadByte(CTRL_REG5_G);
+  gWriteByte(CTRL_REG5_G, c & ~0x40);  // Disable gyro FIFO  
+  wait_ms(20);
+  gWriteByte(FIFO_CTRL_REG_G, 0x00);   // Enable gyro bypass mode
+  
+  //  Now get the accelerometer biases
+  c = xmReadByte(CTRL_REG0_XM);
+  xmWriteByte(CTRL_REG0_XM, c | 0x40);      // Enable accelerometer FIFO  
+  wait_ms(20);                                // Wait for change to take effect
+  xmWriteByte(FIFO_CTRL_REG, 0x20 | 0x1F);  // Enable accelerometer FIFO stream mode and set watermark at 32 samples
+  wait_ms(1000);  // delay 1000 milliseconds to collect FIFO samples
+
+  samples = (xmReadByte(FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples
+
+   for(ii = 0; ii < samples ; ii++) {          // Read the accelerometer data stored in the FIFO
+   
+    data[0] = xmReadByte(OUT_X_L_A);
+    data[1] = xmReadByte(OUT_X_H_A);
+    data[2] = xmReadByte(OUT_Y_L_A);
+    data[3] = xmReadByte(OUT_Y_H_A);
+    data[4] = xmReadByte(OUT_Z_L_A);
+    data[5] = xmReadByte(OUT_Z_H_A);
+    accel_bias[0] += (((int16_t)data[1] << 8) | data[0]);
+    accel_bias[1] += (((int16_t)data[3] << 8) | data[2]);
+    accel_bias[2] += (((int16_t)data[5] << 8) | data[4]) - (int16_t)(1./aRes); // Assumes sensor facing up!
+  }  
+
+  accel_bias[0] /= samples; // average the data
+  accel_bias[1] /= samples; 
+  accel_bias[2] /= samples; 
+  
+  abias[0] = (float)accel_bias[0]*aRes; // Properly scale data to get gs
+  abias[1] = (float)accel_bias[1]*aRes;
+  abias[2] = (float)accel_bias[2]*aRes;
+
+  c = xmReadByte(CTRL_REG0_XM);
+  xmWriteByte(CTRL_REG0_XM, c & ~0x40);    // Disable accelerometer FIFO  
+  wait_ms(20);
+  xmWriteByte(FIFO_CTRL_REG, 0x00);       // Enable accelerometer bypass mode
+  
+}
+void LSM9DS0::readAccel()
+{
+  uint16_t Temp = 0;
+  
+  //Get x
+  Temp = xmReadByte(OUT_X_H_A);
+  Temp = Temp<<8;
+  Temp |= xmReadByte(OUT_X_L_A);
+  ax = Temp;
+  
+  
+  //Get y
+  Temp=0;
+  Temp = xmReadByte(OUT_Y_H_A);
+  Temp = Temp<<8;
+  Temp |= xmReadByte(OUT_Y_L_A);
+  ay = Temp;
+  
+  //Get z
+  Temp=0;
+  Temp = xmReadByte(OUT_Z_H_A);
+  Temp = Temp<<8;
+  Temp |= xmReadByte(OUT_Z_L_A);
+  az = Temp;
+  
+}
+
+void LSM9DS0::readMag()
+{
+  uint16_t Temp = 0;  
+
+  //Get x
+  Temp = xmReadByte(OUT_X_H_M);
+  Temp = Temp<<8;
+  Temp |= xmReadByte(OUT_X_L_M);
+  mx = Temp;
+  
+  
+  //Get y
+  Temp=0;
+  Temp = xmReadByte(OUT_Y_H_M);
+  Temp = Temp<<8;
+  Temp |= xmReadByte(OUT_Y_L_M);
+  my = Temp;
+  
+  //Get z
+  Temp=0;
+  Temp = xmReadByte(OUT_Z_H_M);
+  Temp = Temp<<8;
+  Temp |= xmReadByte(OUT_Z_L_M);
+  mz = Temp;
+}
+
+void LSM9DS0::readTemp()
+{
+    uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp  
+    
+    temp[0] = xmReadByte(OUT_TEMP_L_XM);
+    temp[1] = xmReadByte(OUT_TEMP_H_XM);
+    
+    temperature = (((int16_t) temp[1] << 12) | temp[0] << 4 ) >> 4; // Temperature is a 12-bit signed integer
+}
+
+
+void LSM9DS0::readGyro()
+{   
+  uint16_t Temp = 0;
+
+  //Get x
+  Temp = gReadByte(OUT_X_H_G);
+  Temp = Temp<<8;
+  Temp |= gReadByte(OUT_X_L_G);
+  gx = Temp;
+  
+  
+  //Get y
+  Temp=0;
+  Temp = gReadByte(OUT_Y_H_G);
+  Temp = Temp<<8;
+  Temp |= gReadByte(OUT_Y_L_G);
+  gy = Temp;
+  
+  //Get z
+  Temp=0;
+  Temp = gReadByte(OUT_Z_H_G);
+  Temp = Temp<<8;
+  Temp |= gReadByte(OUT_Z_L_G);
+  gz = Temp;
+}
+
+float LSM9DS0::calcGyro(int16_t gyro)
+{
+    // Return the gyro raw reading times our pre-calculated DPS / (ADC tick):
+    return gRes * gyro; 
+}
+
+float LSM9DS0::calcAccel(int16_t accel)
+{
+    // Return the accel raw reading times our pre-calculated g's / (ADC tick):
+    return aRes * accel;
+}
+
+float LSM9DS0::calcMag(int16_t mag)
+{
+    // Return the mag raw reading times our pre-calculated Gs / (ADC tick):
+    return mRes * mag;
+}
+
+void LSM9DS0::setGyroScale(gyro_scale gScl)
+{
+    // We need to preserve the other bytes in CTRL_REG4_G. So, first read it:
+    uint8_t temp = gReadByte(CTRL_REG4_G);
+    // Then mask out the gyro scale bits:
+    temp &= 0xFF^(0x3 << 4);
+    // Then shift in our new scale bits:
+    temp |= gScl << 4;
+    // And write the new register value back into CTRL_REG4_G:
+    gWriteByte(CTRL_REG4_G, temp);
+    
+    // We've updated the sensor, but we also need to update our class variables
+    // First update gScale:
+    gScale = gScl;
+    // Then calculate a new gRes, which relies on gScale being set correctly:
+    calcgRes();
+}
+
+void LSM9DS0::setAccelScale(accel_scale aScl)
+{
+    // We need to preserve the other bytes in CTRL_REG2_XM. So, first read it:
+    uint8_t temp = xmReadByte(CTRL_REG2_XM);
+    // Then mask out the accel scale bits:
+    temp &= 0xFF^(0x3 << 3);
+    // Then shift in our new scale bits:
+    temp |= aScl << 3;
+    // And write the new register value back into CTRL_REG2_XM:
+    xmWriteByte(CTRL_REG2_XM, temp);
+    
+    // We've updated the sensor, but we also need to update our class variables
+    // First update aScale:
+    aScale = aScl;
+    // Then calculate a new aRes, which relies on aScale being set correctly:
+    calcaRes();
+}
+
+void LSM9DS0::setMagScale(mag_scale mScl)
+{
+    // We need to preserve the other bytes in CTRL_REG6_XM. So, first read it:
+    uint8_t temp = xmReadByte(CTRL_REG6_XM);
+    // Then mask out the mag scale bits:
+    temp &= 0xFF^(0x3 << 5);
+    // Then shift in our new scale bits:
+    temp |= mScl << 5;
+    // And write the new register value back into CTRL_REG6_XM:
+    xmWriteByte(CTRL_REG6_XM, temp);
+    
+    // We've updated the sensor, but we also need to update our class variables
+    // First update mScale:
+    mScale = mScl;
+    // Then calculate a new mRes, which relies on mScale being set correctly:
+    calcmRes();
+}
+
+void LSM9DS0::setGyroODR(gyro_odr gRate)
+{
+    // We need to preserve the other bytes in CTRL_REG1_G. So, first read it:
+    uint8_t temp = gReadByte(CTRL_REG1_G);
+    // Then mask out the gyro ODR bits:
+    temp &= 0xFF^(0xF << 4);
+    // Then shift in our new ODR bits:
+    temp |= (gRate << 4);
+    // And write the new register value back into CTRL_REG1_G:
+    gWriteByte(CTRL_REG1_G, temp);
+}
+void LSM9DS0::setAccelODR(accel_odr aRate)
+{
+    // We need to preserve the other bytes in CTRL_REG1_XM. So, first read it:
+    uint8_t temp = xmReadByte(CTRL_REG1_XM);
+    // Then mask out the accel ODR bits:
+    temp &= 0xFF^(0xF << 4);
+    // Then shift in our new ODR bits:
+    temp |= (aRate << 4);
+    // And write the new register value back into CTRL_REG1_XM:
+    xmWriteByte(CTRL_REG1_XM, temp);
+}
+void LSM9DS0::setMagODR(mag_odr mRate)
+{
+    // We need to preserve the other bytes in CTRL_REG5_XM. So, first read it:
+    uint8_t temp = xmReadByte(CTRL_REG5_XM);
+    // Then mask out the mag ODR bits:
+    temp &= 0xFF^(0x7 << 2);
+    // Then shift in our new ODR bits:
+    temp |= (mRate << 2);
+    // And write the new register value back into CTRL_REG5_XM:
+    xmWriteByte(CTRL_REG5_XM, temp);
+}
+
+void LSM9DS0::configGyroInt(uint8_t int1Cfg, uint16_t int1ThsX, uint16_t int1ThsY, uint16_t int1ThsZ, uint8_t duration)
+{
+    gWriteByte(INT1_CFG_G, int1Cfg);
+    gWriteByte(INT1_THS_XH_G, (int1ThsX & 0xFF00) >> 8);
+    gWriteByte(INT1_THS_XL_G, (int1ThsX & 0xFF));
+    gWriteByte(INT1_THS_YH_G, (int1ThsY & 0xFF00) >> 8);
+    gWriteByte(INT1_THS_YL_G, (int1ThsY & 0xFF));
+    gWriteByte(INT1_THS_ZH_G, (int1ThsZ & 0xFF00) >> 8);
+    gWriteByte(INT1_THS_ZL_G, (int1ThsZ & 0xFF));
+    if (duration)
+        gWriteByte(INT1_DURATION_G, 0x80 | duration);
+    else
+        gWriteByte(INT1_DURATION_G, 0x00);
+}
+
+void LSM9DS0::calcgRes()
+{
+    // Possible gyro scales (and their register bit settings) are:
+    // 245 DPS (00), 500 DPS (01), 2000 DPS (10). Here's a bit of an algorithm
+    // to calculate DPS/(ADC tick) based on that 2-bit value:
+    switch (gScale)
+    {
+    case G_SCALE_245DPS:
+        gRes = 245.0 / 32768.0;
+        break;
+    case G_SCALE_500DPS:
+        gRes = 500.0 / 32768.0;
+        break;
+    case G_SCALE_2000DPS:
+        gRes = 2000.0 / 32768.0;
+        break;
+    }
+}
+
+void LSM9DS0::calcaRes()
+{
+    // Possible accelerometer scales (and their register bit settings) are:
+    // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an 
+    // algorithm to calculate g/(ADC tick) based on that 3-bit value:
+    aRes = aScale == A_SCALE_16G ? 16.0 / 32768.0 : 
+           (((float) aScale + 1.0) * 2.0) / 32768.0;
+}
+
+void LSM9DS0::calcmRes()
+{
+    // Possible magnetometer scales (and their register bit settings) are:
+    // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11). Here's a bit of an algorithm
+    // to calculate Gs/(ADC tick) based on that 2-bit value:
+    mRes = mScale == M_SCALE_2GS ? 2.0 / 32768.0 : 
+           (float) (mScale << 2) / 32768.0;
+}
+    
+void LSM9DS0::gWriteByte(uint8_t subAddress, uint8_t data)
+{
+    // Whether we're using I2C or SPI, write a byte using the
+    // gyro-specific I2C address or SPI CS pin.
+    I2CwriteByte(gAddress, subAddress, data);
+}
+
+void LSM9DS0::xmWriteByte(uint8_t subAddress, uint8_t data)
+{
+    // Whether we're using I2C or SPI, write a byte using the
+    // accelerometer-specific I2C address or SPI CS pin.
+        return I2CwriteByte(xmAddress, subAddress, data);
+}
+
+uint8_t LSM9DS0::gReadByte(uint8_t subAddress)
+{
+        return I2CreadByte(gAddress, subAddress);
+}
+
+void LSM9DS0::gReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
+{
+    // Whether we're using I2C or SPI, read multiple bytes using the
+    // gyro-specific I2C address.
+        I2CreadBytes(gAddress, subAddress, dest, count);
+}
+
+uint8_t LSM9DS0::xmReadByte(uint8_t subAddress)
+{
+    // Whether we're using I2C or SPI, read a byte using the
+    // accelerometer-specific I2C address.
+        return I2CreadByte(xmAddress, subAddress);
+}
+
+void LSM9DS0::xmReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
+{
+    // read multiple bytes using the
+    // accelerometer-specific I2C address.
+    I2CreadBytes(xmAddress, subAddress, dest, count);
+}
+
+
+void LSM9DS0::I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data)
+{   
+    i2c_->writeByte(address,subAddress,data);
+}
+
+uint8_t LSM9DS0::I2CreadByte(uint8_t address, uint8_t subAddress)
+{
+    char data[1]; // `data` will store the register data
+    
+    I2CreadBytes(address, subAddress,(uint8_t*)data, 1);
+    return (uint8_t)data[0];
+
+}
+
+void LSM9DS0::I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest,
+                            uint8_t count)
+{   
+    i2c_->readBytes(address, subAddress, count, dest);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LSM9DS0.h	Tue Dec 09 01:15:37 2014 +0000
@@ -0,0 +1,473 @@
+//Most of the Credit goes to jimblom
+#ifndef _LSM9DS0_H__
+#define _LSM9DS0_H__
+
+#include "mbed.h"
+#include "I2Cdev.h"
+
+
+////////////////////////////
+// LSM9DS0 Gyro Registers //
+////////////////////////////
+#define WHO_AM_I_G          0x0F
+#define CTRL_REG1_G         0x20
+#define CTRL_REG2_G         0x21
+#define CTRL_REG3_G         0x22
+#define CTRL_REG4_G         0x23
+#define CTRL_REG5_G         0x24
+#define REFERENCE_G         0x25
+#define STATUS_REG_G        0x27
+#define OUT_X_L_G           0x28
+#define OUT_X_H_G           0x29
+#define OUT_Y_L_G           0x2A
+#define OUT_Y_H_G           0x2B
+#define OUT_Z_L_G           0x2C
+#define OUT_Z_H_G           0x2D
+#define FIFO_CTRL_REG_G     0x2E
+#define FIFO_SRC_REG_G      0x2F
+#define INT1_CFG_G          0x30
+#define INT1_SRC_G          0x31
+#define INT1_THS_XH_G       0x32
+#define INT1_THS_XL_G       0x33
+#define INT1_THS_YH_G       0x34
+#define INT1_THS_YL_G       0x35
+#define INT1_THS_ZH_G       0x36
+#define INT1_THS_ZL_G       0x37
+#define INT1_DURATION_G     0x38
+
+//////////////////////////////////////////
+// LSM9DS0 Accel/Magneto (XM) Registers //
+//////////////////////////////////////////
+#define OUT_TEMP_L_XM       0x05
+#define OUT_TEMP_H_XM       0x06
+#define STATUS_REG_M        0x07
+#define OUT_X_L_M           0x08
+#define OUT_X_H_M           0x09
+#define OUT_Y_L_M           0x0A
+#define OUT_Y_H_M           0x0B
+#define OUT_Z_L_M           0x0C
+#define OUT_Z_H_M           0x0D
+#define WHO_AM_I_XM         0x0F
+#define INT_CTRL_REG_M      0x12
+#define INT_SRC_REG_M       0x13
+#define INT_THS_L_M         0x14
+#define INT_THS_H_M         0x15
+#define OFFSET_X_L_M        0x16
+#define OFFSET_X_H_M        0x17
+#define OFFSET_Y_L_M        0x18
+#define OFFSET_Y_H_M        0x19
+#define OFFSET_Z_L_M        0x1A
+#define OFFSET_Z_H_M        0x1B
+#define REFERENCE_X         0x1C
+#define REFERENCE_Y         0x1D
+#define REFERENCE_Z         0x1E
+#define CTRL_REG0_XM        0x1F
+#define CTRL_REG1_XM        0x20
+#define CTRL_REG2_XM        0x21
+#define CTRL_REG3_XM        0x22
+#define CTRL_REG4_XM        0x23
+#define CTRL_REG5_XM        0x24
+#define CTRL_REG6_XM        0x25
+#define CTRL_REG7_XM        0x26
+#define STATUS_REG_A        0x27
+#define OUT_X_L_A           0x28
+#define OUT_X_H_A           0x29
+#define OUT_Y_L_A           0x2A
+#define OUT_Y_H_A           0x2B
+#define OUT_Z_L_A           0x2C
+#define OUT_Z_H_A           0x2D
+#define FIFO_CTRL_REG       0x2E
+#define FIFO_SRC_REG        0x2F
+#define INT_GEN_1_REG       0x30
+#define INT_GEN_1_SRC       0x31
+#define INT_GEN_1_THS       0x32
+#define INT_GEN_1_DURATION  0x33
+#define INT_GEN_2_REG       0x34
+#define INT_GEN_2_SRC       0x35
+#define INT_GEN_2_THS       0x36
+#define INT_GEN_2_DURATION  0x37
+#define CLICK_CFG           0x38
+#define CLICK_SRC           0x39
+#define CLICK_THS           0x3A
+#define TIME_LIMIT          0x3B
+#define TIME_LATENCY        0x3C
+#define TIME_WINDOW         0x3D
+#define ACT_THS             0x3E
+#define ACT_DUR             0x3F
+
+
+class LSM9DS0
+{
+public:
+    // gyro_scale defines the possible full-scale ranges of the gyroscope:
+    enum gyro_scale
+    {
+        G_SCALE_245DPS,     // 00: +/- 245 degrees per second
+        G_SCALE_500DPS,     // 01: +/- 500 dps
+        G_SCALE_2000DPS,    // 10: +/- 2000 dps
+    };
+    // accel_scale defines all possible FSR's of the accelerometer:
+    enum accel_scale
+    {
+        A_SCALE_2G, // 000: +/- 2g
+        A_SCALE_4G, // 001: +/- 4g
+        A_SCALE_6G, // 010: +/- 6g
+        A_SCALE_8G, // 011: +/- 8g
+        A_SCALE_16G // 100: +/- 16g
+    };
+    // mag_scale defines all possible FSR's of the magnetometer:
+    enum mag_scale
+    {
+        M_SCALE_2GS,    // 00: +/- 2Gs
+        M_SCALE_4GS,    // 01: +/- 4Gs
+        M_SCALE_8GS,    // 10: +/- 8Gs
+        M_SCALE_12GS,   // 11: +/- 12Gs
+    };
+    // gyro_odr defines all possible data rate/bandwidth combos of the gyro:
+    enum gyro_odr
+    {                           // ODR (Hz) --- Cutoff
+        G_ODR_95_BW_125  = 0x0, //   95         12.5
+        G_ODR_95_BW_25   = 0x1, //   95          25
+        // 0x2 and 0x3 define the same data rate and bandwidth
+        G_ODR_190_BW_125 = 0x4, //   190        12.5
+        G_ODR_190_BW_25  = 0x5, //   190         25
+        G_ODR_190_BW_50  = 0x6, //   190         50
+        G_ODR_190_BW_70  = 0x7, //   190         70
+        G_ODR_380_BW_20  = 0x8, //   380         20
+        G_ODR_380_BW_25  = 0x9, //   380         25
+        G_ODR_380_BW_50  = 0xA, //   380         50
+        G_ODR_380_BW_100 = 0xB, //   380         100
+        G_ODR_760_BW_30  = 0xC, //   760         30
+        G_ODR_760_BW_35  = 0xD, //   760         35
+        G_ODR_760_BW_50  = 0xE, //   760         50
+        G_ODR_760_BW_100 = 0xF, //   760         100
+    };
+    // accel_oder defines all possible output data rates of the accelerometer:
+    enum accel_odr
+    {
+        A_POWER_DOWN,   // Power-down mode (0x0)
+        A_ODR_3125,     // 3.125 Hz (0x1)
+        A_ODR_625,      // 6.25 Hz (0x2)
+        A_ODR_125,      // 12.5 Hz (0x3)
+        A_ODR_25,       // 25 Hz (0x4)
+        A_ODR_50,       // 50 Hz (0x5)
+        A_ODR_100,      // 100 Hz (0x6)
+        A_ODR_200,      // 200 Hz (0x7)
+        A_ODR_400,      // 400 Hz (0x8)
+        A_ODR_800,      // 800 Hz (9)
+        A_ODR_1600      // 1600 Hz (0xA)
+    };
+    // accel_oder defines all possible output data rates of the magnetometer:
+    enum mag_odr
+    {
+        M_ODR_3125, // 3.125 Hz (0x00)
+        M_ODR_625,  // 6.25 Hz (0x01)
+        M_ODR_125,  // 12.5 Hz (0x02)
+        M_ODR_25,   // 25 Hz (0x03)
+        M_ODR_50,   // 50 (0x04)
+        M_ODR_100,  // 100 Hz (0x05)
+    };
+
+    // We'll store the gyro, accel, and magnetometer readings in a series of
+    // public class variables. Each sensor gets three variables -- one for each
+    // axis. Call readGyro(), readAccel(), and readMag() first, before using
+    // these variables!
+    // These values are the RAW signed 16-bit readings from the sensors.
+    int16_t gx, gy, gz; // x, y, and z axis readings of the gyroscope
+    int16_t ax, ay, az; // x, y, and z axis readings of the accelerometer
+    int16_t mx, my, mz; // x, y, and z axis readings of the magnetometer
+    int16_t temperature;
+    float abias[3];
+    float gbias[3];
+
+    
+    // LSM9DS0 -- LSM9DS0 class constructor
+    // The constructor will set up a handful of private variables, and set the
+    // communication mode as well.
+    // Input:
+    //  - interface = Either MODE_SPI or MODE_I2C, whichever you're using
+    //              to talk to the IC.
+    //  - gAddr = If MODE_I2C, this is the I2C address of the gyroscope.
+    //              If MODE_SPI, this is the chip select pin of the gyro (CSG)
+    //  - xmAddr = If MODE_I2C, this is the I2C address of the accel/mag.
+    //              If MODE_SPI, this is the cs pin of the accel/mag (CSXM)
+    LSM9DS0(PinName sda, PinName scl, uint8_t gAddr, uint8_t xmAddr);
+    
+    // begin() -- Initialize the gyro, accelerometer, and magnetometer.
+    // This will set up the scale and output rate of each sensor. It'll also
+    // "turn on" every sensor and every axis of every sensor.
+    // Input:
+    //  - gScl = The scale of the gyroscope. This should be a gyro_scale value.
+    //  - aScl = The scale of the accelerometer. Should be a accel_scale value.
+    //  - mScl = The scale of the magnetometer. Should be a mag_scale value.
+    //  - gODR = Output data rate of the gyroscope. gyro_odr value.
+    //  - aODR = Output data rate of the accelerometer. accel_odr value.
+    //  - mODR = Output data rate of the magnetometer. mag_odr value.
+    // Output: The function will return an unsigned 16-bit value. The most-sig
+    //      bytes of the output are the WHO_AM_I reading of the accel. The
+    //      least significant two bytes are the WHO_AM_I reading of the gyro.
+    // All parameters have a defaulted value, so you can call just "begin()".
+    // Default values are FSR's of: +/- 245DPS, 2g, 2Gs; ODRs of 95 Hz for 
+    // gyro, 100 Hz for accelerometer, 100 Hz for magnetometer.
+    // Use the return value of this function to verify communication.
+    uint16_t begin(gyro_scale gScl = G_SCALE_245DPS, 
+                accel_scale aScl = A_SCALE_2G, mag_scale mScl = M_SCALE_2GS,
+                gyro_odr gODR = G_ODR_95_BW_125, accel_odr aODR = A_ODR_50, 
+                mag_odr mODR = M_ODR_50);
+    
+    // readGyro() -- Read the gyroscope output registers.
+    // This function will read all six gyroscope output registers.
+    // The readings are stored in the class' gx, gy, and gz variables. Read
+    // those _after_ calling readGyro().
+    void readGyro();
+    
+    // readAccel() -- Read the accelerometer output registers.
+    // This function will read all six accelerometer output registers.
+    // The readings are stored in the class' ax, ay, and az variables. Read
+    // those _after_ calling readAccel().
+    void readAccel();
+    
+    // readMag() -- Read the magnetometer output registers.
+    // This function will read all six magnetometer output registers.
+    // The readings are stored in the class' mx, my, and mz variables. Read
+    // those _after_ calling readMag().
+    void readMag();
+    
+    // readTemp() -- Read the temperature output register.
+    // This function will read two temperature output registers.
+    // The combined readings are stored in the class' temperature variables. Read
+    // those _after_ calling readTemp().
+    void readTemp();
+    
+    // calcGyro() -- Convert from RAW signed 16-bit value to degrees per second
+    // This function reads in a signed 16-bit value and returns the scaled
+    // DPS. This function relies on gScale and gRes being correct.
+    // Input:
+    //  - gyro = A signed 16-bit raw reading from the gyroscope.
+    float calcGyro(int16_t gyro);
+    
+    // calcAccel() -- Convert from RAW signed 16-bit value to gravity (g's).
+    // This function reads in a signed 16-bit value and returns the scaled
+    // g's. This function relies on aScale and aRes being correct.
+    // Input:
+    //  - accel = A signed 16-bit raw reading from the accelerometer.
+    float calcAccel(int16_t accel);
+    
+    // calcMag() -- Convert from RAW signed 16-bit value to Gauss (Gs)
+    // This function reads in a signed 16-bit value and returns the scaled
+    // Gs. This function relies on mScale and mRes being correct.
+    // Input:
+    //  - mag = A signed 16-bit raw reading from the magnetometer.
+    float calcMag(int16_t mag);
+    
+    // setGyroScale() -- Set the full-scale range of the gyroscope.
+    // This function can be called to set the scale of the gyroscope to 
+    // 245, 500, or 200 degrees per second.
+    // Input:
+    //  - gScl = The desired gyroscope scale. Must be one of three possible
+    //      values from the gyro_scale enum.
+    void setGyroScale(gyro_scale gScl);
+    
+    // setAccelScale() -- Set the full-scale range of the accelerometer.
+    // This function can be called to set the scale of the accelerometer to
+    // 2, 4, 6, 8, or 16 g's.
+    // Input:
+    //  - aScl = The desired accelerometer scale. Must be one of five possible
+    //      values from the accel_scale enum.
+    void setAccelScale(accel_scale aScl);
+    
+    // setMagScale() -- Set the full-scale range of the magnetometer.
+    // This function can be called to set the scale of the magnetometer to
+    // 2, 4, 8, or 12 Gs.
+    // Input:
+    //  - mScl = The desired magnetometer scale. Must be one of four possible
+    //      values from the mag_scale enum.
+    void setMagScale(mag_scale mScl);
+    
+    // setGyroODR() -- Set the output data rate and bandwidth of the gyroscope
+    // Input:
+    //  - gRate = The desired output rate and cutoff frequency of the gyro.
+    //      Must be a value from the gyro_odr enum (check above, there're 14).
+    void setGyroODR(gyro_odr gRate);
+    
+    // setAccelODR() -- Set the output data rate of the accelerometer
+    // Input:
+    //  - aRate = The desired output rate of the accel.
+    //      Must be a value from the accel_odr enum (check above, there're 11).
+    void setAccelODR(accel_odr aRate);
+    
+    // setMagODR() -- Set the output data rate of the magnetometer
+    // Input:
+    //  - mRate = The desired output rate of the mag.
+    //      Must be a value from the mag_odr enum (check above, there're 6).
+    void setMagODR(mag_odr mRate);
+    
+    // configGyroInt() -- Configure the gyro interrupt output.
+    // Triggers can be set to either rising above or falling below a specified
+    // threshold. This function helps setup the interrupt configuration and 
+    // threshold values for all axes.
+    // Input:
+    //  - int1Cfg = A 8-bit value that is sent directly to the INT1_CFG_G
+    //      register. This sets AND/OR and high/low interrupt gen for each axis
+    //  - int1ThsX = 16-bit interrupt threshold value for x-axis
+    //  - int1ThsY = 16-bit interrupt threshold value for y-axis
+    //  - int1ThsZ = 16-bit interrupt threshold value for z-axis
+    //  - duration = Duration an interrupt holds after triggered. This value
+    //      is copied directly into the INT1_DURATION_G register.
+    // Before using this function, read about the INT1_CFG_G register and
+    // the related INT1* registers in the LMS9DS0 datasheet.
+    void configGyroInt(uint8_t int1Cfg, uint16_t int1ThsX = 0,
+                          uint16_t int1ThsY = 0, uint16_t int1ThsZ = 0, 
+                          uint8_t duration = 0);
+                          
+    void calLSM9DS0(float gbias[3], float abias[3]);
+
+
+private:    
+    // xmAddress and gAddress store the I2C address
+    // for each sensor.
+    uint8_t xmAddress, gAddress;
+    
+    // gScale, aScale, and mScale store the current scale range for each 
+    // sensor. Should be updated whenever that value changes.
+    gyro_scale gScale;
+    accel_scale aScale;
+    mag_scale mScale;
+    
+    // gRes, aRes, and mRes store the current resolution for each sensor. 
+    // Units of these values would be DPS (or g's or Gs's) per ADC tick.
+    // This value is calculated as (sensor scale) / (2^15).
+    float gRes, aRes, mRes;
+    
+    // initGyro() -- Sets up the gyroscope to begin reading.
+    // This function steps through all five gyroscope control registers.
+    // Upon exit, the following parameters will be set:
+    //  - CTRL_REG1_G = 0x0F: Normal operation mode, all axes enabled. 
+    //      95 Hz ODR, 12.5 Hz cutoff frequency.
+    //  - CTRL_REG2_G = 0x00: HPF set to normal mode, cutoff frequency
+    //      set to 7.2 Hz (depends on ODR).
+    //  - CTRL_REG3_G = 0x88: Interrupt enabled on INT_G (set to push-pull and
+    //      active high). Data-ready output enabled on DRDY_G.
+    //  - CTRL_REG4_G = 0x00: Continuous update mode. Data LSB stored in lower
+    //      address. Scale set to 245 DPS. SPI mode set to 4-wire.
+    //  - CTRL_REG5_G = 0x00: FIFO disabled. HPF disabled.
+    void initGyro();
+    
+    // initAccel() -- Sets up the accelerometer to begin reading.
+    // This function steps through all accelerometer related control registers.
+    // Upon exit these registers will be set as:
+    //  - CTRL_REG0_XM = 0x00: FIFO disabled. HPF bypassed. Normal mode.
+    //  - CTRL_REG1_XM = 0x57: 100 Hz data rate. Continuous update.
+    //      all axes enabled.
+    //  - CTRL_REG2_XM = 0x00: +/- 2g scale. 773 Hz anti-alias filter BW.
+    //  - CTRL_REG3_XM = 0x04: Accel data ready signal on INT1_XM pin.
+    void initAccel();
+    
+    // initMag() -- Sets up the magnetometer to begin reading.
+    // This function steps through all magnetometer-related control registers.
+    // Upon exit these registers will be set as:
+    //  - CTRL_REG4_XM = 0x04: Mag data ready signal on INT2_XM pin.
+    //  - CTRL_REG5_XM = 0x14: 100 Hz update rate. Low resolution. Interrupt
+    //      requests don't latch. Temperature sensor disabled.
+    //  - CTRL_REG6_XM = 0x00: +/- 2 Gs scale.
+    //  - CTRL_REG7_XM = 0x00: Continuous conversion mode. Normal HPF mode.
+    //  - INT_CTRL_REG_M = 0x09: Interrupt active-high. Enable interrupts.
+    void initMag();
+    
+    // gReadByte() -- Reads a byte from a specified gyroscope register.
+    // Input:
+    //  - subAddress = Register to be read from.
+    // Output:
+    //  - An 8-bit value read from the requested address.
+    uint8_t gReadByte(uint8_t subAddress);
+    
+    // gReadBytes() -- Reads a number of bytes -- beginning at an address
+    // and incrementing from there -- from the gyroscope.
+    // Input:
+    //  - subAddress = Register to be read from.
+    //  - * dest = A pointer to an array of uint8_t's. Values read will be
+    //      stored in here on return.
+    //  - count = The number of bytes to be read.
+    // Output: No value is returned, but the `dest` array will store
+    //  the data read upon exit.
+    void gReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count);
+    
+    // gWriteByte() -- Write a byte to a register in the gyroscope.
+    // Input:
+    //  - subAddress = Register to be written to.
+    //  - data = data to be written to the register.
+    void gWriteByte(uint8_t subAddress, uint8_t data);
+    
+    // xmReadByte() -- Read a byte from a register in the accel/mag sensor
+    // Input:
+    //  - subAddress = Register to be read from.
+    // Output:
+    //  - An 8-bit value read from the requested register.
+    uint8_t xmReadByte(uint8_t subAddress);
+    
+    // xmReadBytes() -- Reads a number of bytes -- beginning at an address
+    // and incrementing from there -- from the accelerometer/magnetometer.
+    // Input:
+    //  - subAddress = Register to be read from.
+    //  - * dest = A pointer to an array of uint8_t's. Values read will be
+    //      stored in here on return.
+    //  - count = The number of bytes to be read.
+    // Output: No value is returned, but the `dest` array will store
+    //  the data read upon exit.
+    void xmReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count);
+    
+    // xmWriteByte() -- Write a byte to a register in the accel/mag sensor.
+    // Input:
+    //  - subAddress = Register to be written to.
+    //  - data = data to be written to the register.
+    void xmWriteByte(uint8_t subAddress, uint8_t data);
+    
+    // calcgRes() -- Calculate the resolution of the gyroscope.
+    // This function will set the value of the gRes variable. gScale must
+    // be set prior to calling this function.
+    void calcgRes();
+    
+    // calcmRes() -- Calculate the resolution of the magnetometer.
+    // This function will set the value of the mRes variable. mScale must
+    // be set prior to calling this function.
+    void calcmRes();
+    
+    // calcaRes() -- Calculate the resolution of the accelerometer.
+    // This function will set the value of the aRes variable. aScale must
+    // be set prior to calling this function.
+    void calcaRes();
+    
+    
+    ///////////////////
+    // I2C Functions //
+    ///////////////////
+    I2Cdev* i2c_;
+
+    
+    // I2CwriteByte() -- Write a byte out of I2C to a register in the device
+    // Input:
+    //  - address = The 7-bit I2C address of the slave device.
+    //  - subAddress = The register to be written to.
+    //  - data = Byte to be written to the register.
+    void I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data);
+    
+    // I2CreadByte() -- Read a single byte from a register over I2C.
+    // Input:
+    //  - address = The 7-bit I2C address of the slave device.
+    //  - subAddress = The register to be read from.
+    // Output:
+    //  - The byte read from the requested address.
+    uint8_t I2CreadByte(uint8_t address, uint8_t subAddress);
+    
+    // I2CreadBytes() -- Read a series of bytes, starting at a register via SPI
+    // Input:
+    //  - address = The 7-bit I2C address of the slave device.
+    //  - subAddress = The register to begin reading.
+    //  - * dest = Pointer to an array where we'll store the readings.
+    //  - count = Number of registers to be read.
+    // Output: No value is returned by the function, but the registers read are
+    //      all stored in the *dest array given.
+    void I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count);
+};
+
+#endif // _LSM9DS0_H //

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.cpp	Tue Dec 09 01:15:37 2014 +0000
@@ -0,0 +1,105 @@
+#include "Robot.h"
+
+Robot::Robot()
+{
+    //Init member variables to null
+    host   = 0;
+    AckLED = 0;
+}
+
+Robot::Robot(Serial *MbedSerial, DigitalOut *ConfirmationLed)
+{
+    //Init member variables
+    host   = MbedSerial;
+    AckLED = ConfirmationLed;
+    
+    //Setup serial
+    host->baud(19200);  
+
+}
+
+bool Robot::SendCommand(string arg1)
+{   
+    if(host != 0)
+    {
+       for(int i =0; i < arg1.length(); i++)
+       {
+          host->putc(arg1[i]);
+       }
+       
+       host->putc('\r');
+       return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+bool Robot::SetMotorVelocity(int velocity)
+{
+   char VelocityCommand[25];
+   
+   sprintf(VelocityCommand, "mogo 1:%d 2:%d", velocity, velocity); 
+   
+   return SendCommand(VelocityCommand);   
+}
+
+bool Robot::SetSelectMotorVelocity(int LEFT, int RIGHT)
+{
+   char VelocityCommand[25];
+   
+   sprintf(VelocityCommand, "mogo 1:%d 2:%d", LEFT, RIGHT); 
+   
+   return SendCommand(string(VelocityCommand));
+}
+
+bool Robot::StopRobot()
+{
+   return SendCommand(string("stop"));
+}
+
+bool Robot::SetMotorPWM(int ramp, int PWMValue)
+{
+    char PWMCommand[25];
+    
+    if( ramp > 0)
+    sprintf(PWMCommand, "pwm r:%d 1:%d 2:%d", ramp, PWMValue, PWMValue); 
+    else
+    sprintf(PWMCommand, "pwm 1:%d 2:%d", PWMValue, PWMValue); 
+    
+    return SendCommand(string(PWMCommand));
+}
+
+ 
+bool Robot::SetSelectMotorPWM(int MotorNumber,int ramp, int PWMValue)
+{
+    char PWMCommand[25];
+    
+    if( ramp > 0)
+    sprintf(PWMCommand, "pwm r:%d %d:%d", ramp, MotorNumber, PWMValue); 
+    else
+    sprintf(PWMCommand, "pwm %d:%d", MotorNumber, PWMValue); 
+    
+    return SendCommand(string(PWMCommand));
+}
+
+bool Robot::WaitForAck()
+{
+    char Output[4];
+    int i=0;
+    
+    while(!host->readable()) {;} //spin unto we get something
+    
+    //Get all data out of buffer
+    while(host->readable()) { Output[i++] = (host->getc());
+    
+        if(i == 4) break; 
+     }
+     
+     //Return true  only if we have an ACK
+     if(Output[0] == 'A' && Output[1] == 'C')
+        return true;
+     else
+       return false;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.h	Tue Dec 09 01:15:37 2014 +0000
@@ -0,0 +1,25 @@
+#include "mbed.h"
+//#include "USBHost.h"
+//#include "Utils.h"
+#include <string>
+
+class Robot
+{
+public:
+     Robot(); //Constructor
+     Robot(Serial *MbedSerial, DigitalOut *ConfirmationLed); //Real constructor
+     bool SetMotorVelocity(int velocity);                         //SetMotorVelocity
+     bool SetSelectMotorVelocity(int LEFT, int RIGHT);  //Set individual Motor Velocity
+     bool StopRobot();                                            //Send the stop command
+     bool SetMotorPWM(int ramp, int PWMValue);                    //Set Motor PWM
+     bool SetSelectMotorPWM(int MotorNumber,int ramp, int PWMValue);    //Set individual Motor PWM
+     bool WaitForAck();
+
+private:
+     bool SendCommand(string arg1);                               //Send any properly formated string
+
+     Serial* host;                                          //points to a serial connection
+     DigitalOut* AckLED;                                    //Keeps track of if the mbed received an ack
+    
+};
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VelocityTracker.cpp	Tue Dec 09 01:15:37 2014 +0000
@@ -0,0 +1,50 @@
+#include "mbed.h"
+#include "math.h"
+
+#include "LSM9DS0.h"
+#include "VelocityTracker.h"
+
+// SDO_XM and SDO_G are both grounded, so our addresses are:
+#define LSM9DS0_XM  0x1D // Would be 0x1E if SDO_XM is LOW
+#define LSM9DS0_G   0x6B // Would be 0x6A if SDO_G is LOW
+
+VelocityTracker::VelocityTracker(PinName sda, PinName scl, float updateRate, float biasRate) {
+    imu = new LSM9DS0(sda, scl, LSM9DS0_G, LSM9DS0_XM);
+    _updateRate = updateRate;
+    _biasRate = biasRate;
+    /*
+    biasTicker.attach(&this, &VelocityTracker::updateBias, _biasRate);
+    updateTicker.attach(&this, &VelocityTracker::updateVelocity, _updateRate);
+    */
+    _bias[0] = 0;
+    _bias[1] = 0;
+    _bias[2] = 0;
+}
+
+void VelocityTracker::updateBias() {
+    imu->readAccel();
+    _bias[0] += imu->ax;
+    _bias[1] += imu->ay;
+    _bias[2] += imu->az;
+    sampleCount++;
+}
+
+void VelocityTracker::updateVelocity() {
+    float ax = _bias[0] / sampleCount;
+    float ay = _bias[1] / sampleCount;
+    float az = _bias[2] / sampleCount;
+    
+    // Multiplying by 9.8 to convert from g's
+    _vx += ax * 9.8 * _updateRate;
+    _vy += ay * 9.8 * _updateRate;
+    _vz += az * 9.8 * _updateRate;
+    
+    _bias[0] = 0;
+    _bias[1] = 0;
+    _bias[2] = 0;
+    sampleCount = 0;
+}
+
+float VelocityTracker::getVelocity() {
+    return sqrt(_vx*_vx + _vy*_vy);             // Assuming on a flat surface
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VelocityTracker.h	Tue Dec 09 01:15:37 2014 +0000
@@ -0,0 +1,24 @@
+#include "mbed.h"
+
+#include "LSM9DS0.h"
+
+class VelocityTracker {
+
+public:
+    VelocityTracker(PinName sda, PinName scl, float updateRate, float biasRate);
+    
+    void updateBias();
+    
+    void updateVelocity();
+    
+    float getVelocity();
+    
+private:
+    LSM9DS0 *imu;
+    Ticker biasTicker, updateTicker;
+    float _bias[3];
+    float _vx, _vy, _vz;
+    float _velocity;
+    float _biasRate, _updateRate;
+    int sampleCount;
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Dec 09 01:15:37 2014 +0000
@@ -0,0 +1,160 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "Robot.h"
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+//RFID data from other MBed
+DigitalIn rfIdBit1(p21);
+DigitalIn rfIdBit0(p22);
+InterruptIn rfIdInterrupt(p23);
+
+//Robot code
+Serial device(p9, p10);
+Serial rob(p13, p14);
+Robot TheRobot(&rob, &led1);
+
+
+//Debug
+Serial pc(USBTX, USBRX); // tx, rx
+Mutex pc_mutex;
+
+//Global Variables
+float biasRate = 0.1;
+float updateRate = 2.0;
+float speed_modifier=1;
+float current_speed=0;
+int p1xdir=1,p2xdir=-1; //Direction
+int speed_limit=0;
+
+void limit_detect(void const *args)
+{
+    while(1)
+    {
+        if(current_speed<speed_limit) speed_modifier=1.0;
+        else speed_modifier = speed_limit / current_speed;
+        Thread::wait(20);
+    }
+}
+
+void SetSpeed(void const *args)
+{
+   while(1)
+   {
+      //Set bits
+      int  RfidFlag = -1;
+      int V1 = rfIdBit1? 1:0;
+      int V0 = rfIdBit0? 1:0;
+
+      //Set flag
+      RfidFlag = V1 <<1;
+      RfidFlag |= V0;
+      if(RfidFlag == 0) continue;
+  
+      //Debug
+      //pc_mutex.lock();
+      //pc.printf("%d , %d    ___ %d\n",rfIdBit1? 1:0, rfIdBit0?1:0, RfidFlag);
+      //pc_mutex.unlock();
+   
+      //Set speed limit based on RfidFlag
+      switch(RfidFlag)
+      {
+          case 0:
+              speed_limit =  0;
+              break;
+          case 1:
+              speed_limit =  0;
+              break;
+          case 2:
+              speed_limit = 10;
+              break;
+          case 3:
+              speed_limit = 60;
+              break;
+          default:
+              speed_limit =  0;
+              break;
+           
+    }
+    TheRobot.SetSelectMotorVelocity(0, 0);
+    wait_ms(5);
+    TheRobot.SetSelectMotorVelocity(p1xdir*speed_limit, p2xdir*speed_limit);
+    Thread::wait(500);
+   }
+}
+
+int main() 
+{
+    //Start thread
+    Thread Poll_RFID(SetSpeed);
+    
+    
+    char y1, y2, y3;
+    while(1) 
+    {
+        y3=device.getc();
+        y2=y3 & 0x0F;
+        y1=(y3 & 0xF0)>>4;
+
+        if(y1==0x0C) p1xdir=-1;
+        else if (y1==0x03) p1xdir=1;
+        else p1xdir=0;
+
+        if(y2==0x0C) p2xdir=-1;
+        else if (y2==0x03) p2xdir=1;
+        else p2xdir=0;
+    
+       //TheRobot.SetSelectMotorVelocity(p1xdir*speed_limit, p2xdir*speed_limit);
+
+        /*txflag=txchar & 0xC0;
+        switch(txflag)
+        {
+            case 0x00:
+                JS1x=(int)txchar & 0x3F;
+                break;
+            case 0xC0:
+                JS2x=(int)txchar & 0x3F;
+                break;
+            default:
+                break;
+        }*/
+        
+    
+    
+        //Break info into direction and magnitude
+        /*if(JS1x>31){p1xdir=-1; p1x=JS1x-32;}
+        else {p1xdir=1; p1x=31-JS1x;}
+        if(JS2x>31){p2xdir=1; p2x=JS2x-32;}
+        else {p2xdir=-1; p2x=31-JS2x;}
+        */
+        
+        //current_speed=sqrt((float)p1x*(float)p1x+(float)p2x*(float)p2x);
+        
+        
+        //PMW output based on modifier
+        //pmwL = p1xdir * (speed_modifier * p1x=31-JS;) * 2;
+        //pmwR = p2xdir * (speed_modifier * p2x) * 2;
+        
+        //TheRobot.SetSelectMotorVelocity(pmwL, pmwR);
+        //TheRobot.WaitForAck();
+        //TheRobot.SetSelectMotorVelocity(2, 60);
+        //wait(5);
+        //TheRobot.SetSelectMotorVelocity(1, 15);
+        //wait(5);
+        //TheRobot.SetSelectMotorVelocity(2, pmwR);
+        
+        //DEBUG
+        //if(JS1x>16) led1=1;
+        //else led1=0;
+        /*if(JS1x>32) led2=1;
+        else led2=0;
+        if(JS2x>16) led3=1;
+        else led3=0;
+        if(JS2x>32) led4=1;
+        else led4=0;*/
+    }
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Tue Dec 09 01:15:37 2014 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/mbed_official/code/mbed-rtos/#318e02f48146

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Dec 09 01:15:37 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89
\ No newline at end of file