Revision 7:95e74f827c08, committed 2013-11-02

Comitter:

tyftyftyf

Date:

Sat Nov 02 17:23:43 2013 +0000

Parent:

6:40ac13ef7290

Child:

8:43f4b192e893

Commit message:

Initial Commit

Changed in this revision

--- a/GurvIMU.cpp	Sat Jun 22 11:23:45 2013 +0000
+++ b/GurvIMU.cpp	Sat Nov 02 17:23:43 2013 +0000
@@ -11,7 +11,7 @@
 GurvIMU::GurvIMU()
 {
     //MPU
-    mpu = MPU6050(0x69); //0x69 = MPU6050 I2C ADDRESS
+    mpu = MPU6050(0x68); //0x69 = MPU6050 I2C ADDRESS
 
     // Variable definitions
     q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;  // quaternion of sensor frame relative to auxiliary frame
@@ -32,7 +32,7 @@
     for(int i = 3; i<6; i++) values[i] = (accgyroval[i]-offset[i]) * (M_PI / 180) / 16.4f;
 }
 
-void GurvIMU::getVerticalAcceleration(float av)
+void GurvIMU::getVerticalAcceleration(float *av)
 {
     float values[6];
     float q[4]; // quaternion
@@ -44,7 +44,7 @@
     g_z = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];   
     
     getValues(values);
-    av = g_x*values[0]+g_y*values[1]+g_z*values[2]-offset[2];
+    *av = g_x*values[0]+g_y*values[1]+g_z*values[2]-offset[2];
 }
      
 
@@ -169,19 +169,3 @@
     getOffset();
     wait(0.005);
 }
-
-
-float invSqrt(float number)
-{
-    volatile long i;
-    volatile float x, y;
-    volatile const float f = 1.5F;
-
-    x = number * 0.5F;
-    y = number;
-    i = * ( long * ) &y;
-    i = 0x5f375a86 - ( i >> 1 );
-    y = * ( float * ) &i;
-    y = y * ( f - ( x * y * y ) );
-    return y;
-}
\ No newline at end of file

--- a/I2Cdev.cpp	Sat Jun 22 11:23:45 2013 +0000
+++ b/I2Cdev.cpp	Sat Nov 02 17:23:43 2013 +0000
@@ -5,7 +5,7 @@
 
 #include "I2Cdev.h"
 
-#define useDebugSerial
+//#define useDebugSerial
 
 I2Cdev::I2Cdev(): debugSerial(USBTX, USBRX), i2c(I2C_SDA,I2C_SCL)
 {
@@ -17,6 +17,11 @@
 
 }
 
+I2Cdev::I2Cdev(I2C i2c_): debugSerial(USBTX, USBRX), i2c(i2c_)
+{
+
+}
+
 /** Read a single bit from an 8-bit device register.
  * @param devAddr I2C slave device address
  * @param regAddr Register regAddr to read from
@@ -133,7 +138,7 @@
     command[0] = regAddr;
     char *redData = (char*)malloc(length);
     i2c.write(devAddr<<1, command, 1, true);
-    i2c.read(devAddr<<1, redData, length);
+    i2c.read((devAddr<<1)+1, redData, length);
     for(int i =0; i < length; i++) {
         data[i] = redData[i];
     }

--- a/I2Cdev.h	Sat Jun 22 11:23:45 2013 +0000
+++ b/I2Cdev.h	Sat Nov 02 17:23:43 2013 +0000
@@ -20,6 +20,7 @@
     public:
         I2Cdev();
         I2Cdev(PinName i2cSda, PinName i2cScl);        
+        I2Cdev(I2C i2c_);
         
         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());

--- a/MPU6050.cpp	Sat Jun 22 11:23:45 2013 +0000
+++ b/MPU6050.cpp	Sat Nov 02 17:23:43 2013 +0000
@@ -57,6 +57,11 @@
     devAddr = MPU6050_DEFAULT_ADDRESS;
 }
 
+MPU6050::MPU6050(I2C i2c) : debugSerial(USBTX, USBRX), i2Cdev(i2c)
+{
+    devAddr = MPU6050_DEFAULT_ADDRESS;
+}
+
 /** Specific address constructor.
  * @param address I2C address
  * @see MPU6050_DEFAULT_ADDRESS
@@ -78,8 +83,8 @@
 void MPU6050::initialize()
 {
 
+#ifdef useDebugSerial
     debugSerial.baud(921600); //uses max serial speed
-#ifdef useDebugSerial
     debugSerial.printf("MPU6050::initialize start\n");
 #endif
     setClockSource(MPU6050_CLOCK_PLL_XGYRO);

--- a/MPU6050.h	Sat Jun 22 11:23:45 2013 +0000
+++ b/MPU6050.h	Sat Nov 02 17:23:43 2013 +0000
@@ -408,6 +408,7 @@
         Serial debugSerial;
     public:
         MPU6050();
+        MPU6050(I2C i2c);
         MPU6050(uint8_t address);
 
         void initialize();