Revision 5:472542e91734, committed 2016-08-25

Comitter:

roger5641

Date:

Thu Aug 25 06:14:12 2016 +0000

Parent:

4:29f3711f5f59

Commit message:

ver2; add DC motor

Changed in this revision

diff -r 29f3711f5f59 -r 472542e91734 LSM9DS0.cpp
--- a/LSM9DS0.cpp	Sat Jul 23 13:40:51 2016 +0000
+++ b/LSM9DS0.cpp	Thu Aug 25 06:14:12 2016 +0000
@@ -5,28 +5,28 @@
 Jim Lindblom @ SparkFun Electronics
 Original Creation Date: February 14, 2014 (Happy Valentines Day!)
 https://github.com/sparkfun/LSM9DS0_Breakout
-
+ 
 This file implements all functions of the LSM9DS0 class. Functions here range
 from higher level stuff, like reading/writing LSM9DS0 registers to low-level,
 hardware reads and writes. Both SPI and I2C handler functions can be found
 towards the bottom of this file.
-
+ 
 Development environment specifics:
     IDE: Arduino 1.0.5
     Hardware Platform: Arduino Pro 3.3V/8MHz
     LSM9DS0 Breakout Version: 1.0
-
+ 
 This code is beerware; if you see me (or any other SparkFun employee) at the
 local, and you've found our code helpful, please buy us a round!
-
+ 
 Distributed as-is; no warranty is given.
 ******************************************************************************/
-
+ 
 #include "LSM9DS0.h"
 #include "mbed.h"
-
+ 
 //I2C i2c(D14,D15);
-//SPI spi(D4,D5,D8);
+//SPI spi(D4,D5,D3);
 //****************************************************************************//
 //
 //  LSM9DS0 functions.
@@ -40,8 +40,8 @@
 //  For SPI, construct LSM6DS3 myIMU(SPI_MODE, D9, D6);
 //
 //=================================
-
-LSM9DS0::LSM9DS0(interface_mode interface, uint8_t gAddr, uint8_t xmAddr) : interfaceMode(SPI_MODE), spi_(D4,D5,D3), i2c_(I2C_SDA,I2C_SCL), csG_(D9), csXM_(D6)
+ 
+LSM9DS0::LSM9DS0(interface_mode interface, uint8_t gAddr, uint8_t xmAddr) : interfaceMode(SPI_MODE), spi_(D4,D5,D3), i2c_(I2C_SDA,I2C_SCL), csG_(D12), csXM_(D6)
 {
     // interfaceMode will keep track of whether we're using SPI or I2C:
     interfaceMode = interface;
@@ -51,7 +51,7 @@
     gAddress = gAddr;
     xmAddress = xmAddr;
 }
-
+ 
 uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl, 
                         gyro_odr gODR, accel_odr aODR, mag_odr mODR)
 {
@@ -94,13 +94,13 @@
     setMagScale(mScale); // Set the magnetometer's range.
     
     setGyroOffset(0,0,0);
-    setAccelOffset(-936,-491,265);
+    setAccelOffset(0,0,0);
     setMagOffset(0,0,0);
     
     // Once everything is initialized, return the WHO_AM_I registers we read:
     return (xmTest << 8) | gTest;
 }
-
+ 
 void LSM9DS0::initGyro()
 {
     /* CTRL_REG1_G sets output data rate, bandwidth, power-down and enables
@@ -160,7 +160,7 @@
     // Temporary !!! For testing !!! Remove !!! Or make useful !!!
     configGyroInt(0x2A, 0, 0, 0, 0); // Trigger interrupt when above 0 DPS...
 }
-
+ 
 void LSM9DS0::initAccel()
 {
     /* CTRL_REG0_XM (0x1F) (Default value: 0x00)
@@ -205,7 +205,7 @@
     // Accelerometer data ready on INT1_XM (0x04)
     xmWriteByte(CTRL_REG3_XM, 0x00); 
 }
-
+ 
 void LSM9DS0::initMag()
 {   
     /* CTRL_REG5_XM enables temp sensor, sets mag resolution and data rate
@@ -258,7 +258,7 @@
         0=disable, 1=enable) */
     xmWriteByte(INT_CTRL_REG_M, 0x09); // Enable interrupts for mag, active-low, push-pull
 }
-
+ 
 // This is a function that uses the FIFO to accumulate sample of accelerometer and gyro data, average
 // them, scales them to  gs and deg/s, respectively, and then passes the biases to the main sketch
 // for subtraction from all subsequent data. There are no gyro and accelerometer bias registers to store
@@ -280,14 +280,14 @@
   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
     gReadBytes(OUT_X_L_G,  &data[0], 6);
     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; 
@@ -301,23 +301,23 @@
   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
     xmReadBytes(OUT_X_L_A, &data[0], 6);
     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.0f/aRes); // Assumes sensor facing up!
   }  
-
+ 
   accel_bias[0] /= samples; // average the data
   accel_bias[1] /= samples; 
   accel_bias[2] /= samples; 
@@ -325,13 +325,13 @@
   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
 }
-
+ 
 //**********************
 //  Gyro section
 //**********************
@@ -343,14 +343,14 @@
     gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
     gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz
 }
-
+ 
 void LSM9DS0::setGyroOffset(int16_t _gx, int16_t _gy, int16_t _gz)
 {
     gyroOffset[0] = _gx;
     gyroOffset[1] = _gy;
     gyroOffset[2] = _gz;
 }
-
+ 
 int16_t LSM9DS0::readRawGyroX( void )
 {
     uint8_t temp[2];
@@ -358,7 +358,7 @@
     gx = (temp[1] << 8) | temp[0];
     return gx;
 }
-
+ 
 int16_t LSM9DS0::readRawGyroY( void )
 {
     uint8_t temp[2];
@@ -366,7 +366,7 @@
     gy = (temp[1] << 8) | temp[0];
     return gy;
 }
-
+ 
 int16_t LSM9DS0::readRawGyroZ( void )
 {
     uint8_t temp[2];
@@ -374,25 +374,25 @@
     gz = (temp[1] << 8) | temp[0];
     return gz;
 }
-
+ 
 float LSM9DS0::readFloatGyroX( void )
 {
     float output = calcGyro(readRawGyroX() - gyroOffset[0]);
     return output;
 }
-
+ 
 float LSM9DS0::readFloatGyroY( void )
 {
     float output = calcGyro(readRawGyroY() - gyroOffset[1]);
     return output;
 }
-
+ 
 float LSM9DS0::readFloatGyroZ( void )
 {
     float output = calcGyro(readRawGyroZ() - gyroOffset[2]);
     return output;
 }
-
+ 
 //**********************
 //  Accel section
 //**********************
@@ -404,14 +404,14 @@
     ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay
     az = (temp[5] << 8) | temp[4]; // Store z-axis values into az
 }
-
+ 
 void LSM9DS0::setAccelOffset(int16_t _ax, int16_t _ay, int16_t _az)
 {
     accelOffset[0] = _ax;
     accelOffset[1] = _ay;
     accelOffset[2] = _az;
 }
-
+ 
 int16_t LSM9DS0::readRawAccelX( void )
 {
     uint8_t temp[2];
@@ -419,7 +419,7 @@
     ax = (temp[1] << 8) | temp[0];
     return ax;
 }
-
+ 
 int16_t LSM9DS0::readRawAccelY( void )
 {
     uint8_t temp[2];
@@ -427,7 +427,7 @@
     ay = (temp[1] << 8) | temp[0];
     return ay;
 }
-
+ 
 int16_t LSM9DS0::readRawAccelZ( void )
 {
     uint8_t temp[2];
@@ -435,25 +435,25 @@
     az = (temp[1] << 8) | temp[0];
     return az;
 }
-
+ 
 float LSM9DS0::readFloatAccelX( void )
 {
     float output = calcAccel(readRawAccelX() - accelOffset[0]);
     return output;
 }
-
+ 
 float LSM9DS0::readFloatAccelY( void )
 {
     float output = calcAccel(readRawAccelY() - accelOffset[1]);
     return output;
 }
-
+ 
 float LSM9DS0::readFloatAccelZ( void )
 {
     float output = calcAccel(readRawAccelZ() - accelOffset[2]);
     return output;
 }
-
+ 
 //**********************
 //  Mag section
 //**********************
@@ -465,14 +465,14 @@
     my = (temp[3] << 8) | temp[2]; // Store y-axis values into my
     mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz
 }
-
+ 
 void LSM9DS0::setMagOffset(int16_t _mx, int16_t _my, int16_t _mz)
 {
     magOffset[0] = _mx;
     magOffset[1] = _my;
     magOffset[2] = _mz;
 }
-
+ 
 int16_t LSM9DS0::readRawMagX( void )
 {
     uint8_t temp[2];
@@ -480,7 +480,7 @@
     mx = (temp[1] << 8) | temp[0];
     return mx;
 }
-
+ 
 int16_t LSM9DS0::readRawMagY( void )
 {
     uint8_t temp[2];
@@ -488,7 +488,7 @@
     my = (temp[1] << 8) | temp[0];
     return my;
 }
-
+ 
 int16_t LSM9DS0::readRawMagZ( void )
 {
     uint8_t temp[2];
@@ -496,25 +496,25 @@
     mz = (temp[1] << 8) | temp[0];
     return mz;
 }
-
+ 
 float LSM9DS0::readFloatMagX( void )
 {
     float output = calcMag(readRawMagX() - magOffset[0]);
     return output;
 }
-
+ 
 float LSM9DS0::readFloatMagY( void )
 {
     float output = calcMag(readRawMagY() - magOffset[1]);
     return output;
 }
-
+ 
 float LSM9DS0::readFloatMagZ( void )
 {
     float output = calcMag(readRawMagZ() - magOffset[2]);
     return output;
 }
-
+ 
 //**********************
 //  Temp section
 //**********************
@@ -524,25 +524,25 @@
     xmReadBytes(OUT_TEMP_L_XM, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L_M
     temperature = (((int16_t) temp[1] << 12) | temp[0] << 4 ) >> 4; // Temperature is a 12-bit signed integer
 }
-
+ 
 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:
@@ -560,7 +560,7 @@
     // 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:
@@ -578,7 +578,7 @@
     // 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:
@@ -596,7 +596,7 @@
     // 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:
@@ -641,7 +641,7 @@
     // 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);
@@ -656,7 +656,7 @@
     else
         gWriteByte(INT1_DURATION_G, 0x00);
 }
-
+ 
 void LSM9DS0::calcgRes()
 {
     // Possible gyro scales (and their register bit settings) are:
@@ -675,7 +675,7 @@
         break;
     }
 }
-
+ 
 void LSM9DS0::calcaRes()
 {
     // Possible accelerometer scales (and their register bit settings) are:
@@ -684,7 +684,7 @@
     aRes = aScale == A_SCALE_16G ? 16.0 / 32768.0 : 
            (((float) aScale + 1.0f) * 2.0f) / 32768.0f;
 }
-
+ 
 void LSM9DS0::calcmRes()
 {
     // Possible magnetometer scales (and their register bit settings) are:
@@ -703,7 +703,7 @@
     else if (interfaceMode == SPI_MODE)
         SPIwriteByte(gAddress, subAddress, data);
 }
-
+ 
 void LSM9DS0::xmWriteByte(uint8_t subAddress, uint8_t data)
 {
     // Whether we're using I2C or SPI, write a byte using the
@@ -713,7 +713,7 @@
     else if (interfaceMode == SPI_MODE)
         return SPIwriteByte(xmAddress, subAddress, data);
 }
-
+ 
 uint8_t LSM9DS0::gReadByte(uint8_t subAddress)
 {
     // Whether we're using I2C or SPI, read a byte using the
@@ -725,7 +725,7 @@
     else
         return SPIreadByte(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
@@ -735,7 +735,7 @@
     else if (interfaceMode == SPI_MODE)
         SPIreadBytes(gAddress, subAddress, dest, count);
 }
-
+ 
 uint8_t LSM9DS0::xmReadByte(uint8_t subAddress)
 {
     // Whether we're using I2C or SPI, read a byte using the
@@ -747,7 +747,7 @@
     else
         return SPIreadByte(xmAddress, subAddress);
 }
-
+ 
 void LSM9DS0::xmReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
 {
     // Whether we're using I2C or SPI, read multiple bytes using the
@@ -757,7 +757,7 @@
     else if (interfaceMode == SPI_MODE)
         SPIreadBytes(xmAddress, subAddress, dest, count);
 }
-
+ 
 void LSM9DS0::initSPI()
 {
     csG_ = 1;
@@ -767,7 +767,7 @@
 //    spi_.frequency(1000000);
     spi_.format(8,0b11);
 }
-
+ 
 void LSM9DS0::SPIwriteByte(uint8_t csPin, uint8_t subAddress, uint8_t data)
 {
     // Initiate communication
@@ -784,7 +784,7 @@
     csG_ = 1; // Close communication
     csXM_= 1;
 }
-
+ 
 uint8_t LSM9DS0::SPIreadByte(uint8_t csPin, uint8_t subAddress)
 {
     uint8_t temp;
@@ -793,7 +793,7 @@
     SPIreadBytes(csPin, subAddress, &temp, 1);
     return temp;
 }
-
+ 
 void LSM9DS0::SPIreadBytes(uint8_t csPin, uint8_t subAddress,
                             uint8_t * dest, uint8_t count)
 {
@@ -816,13 +816,13 @@
     csG_ = 1; // Close communication
     csXM_= 1;
 }
-
+ 
 void LSM9DS0::initI2C()
 {
 //    Wire.begin();   // Initialize I2C library
     ;
 }
-
+ 
 // Wire.h read and write protocols
 void LSM9DS0::I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data)
 {
@@ -832,7 +832,7 @@
 //    Wire.write(data);                 // Put data in Tx buffer
 //    Wire.endTransmission();           // Send the Tx buffer
 }
-
+ 
 uint8_t LSM9DS0::I2CreadByte(uint8_t address, uint8_t subAddress)
 {
     return 0;
@@ -844,7 +844,7 @@
 //    data = Wire.read();                      // Fill Rx buffer with result
 //    return data;                             // Return data read from slave register
 }
-
+ 
 void LSM9DS0::I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count)
 {
     ;
@@ -858,4 +858,24 @@
 //    {
 //        dest[i++] = Wire.read(); // Put read results in the Rx buffer
 //    }
-}
\ No newline at end of file
+}
+ 
+void LSM9DS0::complementaryFilter(float * data, float dt)
+{
+    
+    float pitchAcc, rollAcc;
+ 
+    /* Integrate the gyro data(deg/s) over time to get angle */
+    pitch += data[5] * dt;  // Angle around the Z-axis
+    roll +=  data[3] * dt;  // Angle around the X-axis
+    
+    /* Turning around the X-axis results in a vector on the Y-axis
+    whereas turning around the Y-axis results in a vector on the X-axis. */
+    pitchAcc = (float)atan2f(-data[0], -data[1])*180.0f/PI;
+    rollAcc  = (float)atan2f(data[2], -data[1])*180.0f/PI;
+  
+    /* Apply Complementary Filter */
+    pitch = pitch * 0.999 + pitchAcc * 0.001;
+    roll  = roll  * 0.999 + rollAcc  * 0.001;
+}
+            
\ No newline at end of file