Diff: Sensors.cpp

Revision:

2:871b5efb2043

Parent:

1:b44bd62c542f

Child:

3:a3e1a06c486d

--- a/Sensors.cpp	Wed May 20 11:45:38 2015 +0000
+++ b/Sensors.cpp	Sat May 30 12:26:02 2015 +0000
@@ -1,5 +1,6 @@
 #include "Sensors.h"
 
+
 I2C i2c(p29, p28);
 
 Sensors* Sensors::instance = new Sensors();
@@ -42,33 +43,70 @@
 
 void Sensors::setupAngle()
 {
+    //TODO
 }
 
 void Sensors::setupIMU()
 {
-    /* Setup MPU6500 */
+    //Sets sample rate to 8000/1+79 = 100Hz
+    writeRegister(0x19,0x4f);
+    //Disable FSync, 256 DLPF
+    writeRegister(0x1A,0x00);
+    
+    // Disable gyroscope self tests, scale of 200 degrees/s
+    // Disable accelerometer self tests, scale of +-2g, no DHPF
+    cmd[0] = 0x1B;
+    cmd[1] = 0x00;  //bit4 and bit 3 to choose scale range
+    cmd[2] = 0x00;
+    i2c.write(MPU6500,cmd,3);
+    
+    //Freefall threshold of 0mg and duration limit of 0
+    cmd[0] = 0x1D;
+    i2c.write(MPU6500,cmd,3);
+    
+    //Motion threshold of 0mg and duration limit of 0
+    cmd[0] = 0x21;
+    i2c.write(MPU6500,cmd,3);
+    
+    //Disable sensor output FIFO buffer
+    writeRegister(0x23,0x00);
+    
+    //Set aux I2C, from 0x24 to 0x36 = 0x00 
+    
+    //Setup INT pin and AUX I2C pass through
+    cmd[0] = 0x37;
+    cmd[1] = 0x02;
+    cmd[2] = 0x01; 
+    i2c.write(MPU6500,cmd,3);
+    
+    //Reset sensor signal paths
+    writeRegister(0x68,0x00);
+    
+    //Motion detection control
+    writeRegister(0x69,0x00);
+    
+    //Disables FIFO, AUX I2C and I2C reset bits to 0
+    writeRegister(0x6A,0x00);
+    
+    //Sets clock source to gyro reference w/ PLL
+    writeRegister(0x6B,0x02);
+    
+    //Controls frequency of wakeups in accel low power mode plus the sensor standby modes
+    writeRegister(0x6C,0x00);
+
+    //Data transfer to and from the FIFO buffer
+    writeRegister(0x74,0x00);
+    /*
+    
     // Get current value of register 0x6B – Power Management 1 PWR_MGMT_1
     cmd[0] = 0x6B;
-    i2c.write(MPU6500,cmd,1);
-    i2c.read(MPU6500,out,1);
-    int PW = out[0];
-
     // SLEEP = 1, CYCLE = 0, ClOCK = 001, RESET = 0
-    cmd[1] = (PW & 0x10) | 0x41; // OR with 0x1 for SLEEP = 0
-    PW = cmd[1];
+    cmd[1] = (readRegister(0x6B) & 0x10) | 0x41; // OR with 0x1 for SLEEP = 0
     i2c.write(MPU6500,cmd,2);
+    */
+    
+}
 
-    // Set resulution for gyroscope and accelerometer
-    cmd[0] = 0x1B;
-    cmd[1] = 0x00;
-    cmd[2] = 0x00;
-    i2c.write(MPU6500,cmd,3);
-
-    // Set Reset = 0 & Sleep = 0
-    cmd[0] = 0x6B;
-    cmd[1] = PW & 0x3F;
-    i2c.write(MPU6500,cmd,2);
-}
 
 void Sensors::updateIMU()
 {
@@ -77,15 +115,15 @@
     i2c.write(MPU6500,cmd,1,true);
     i2c.read(MPU6500,out,14);
 
-    this->accel_x = (out[0] << 8) + out[1];
-    this->accel_y = (out[2] << 8) + out[3];
-    this->accel_z = (out[4] << 8) + out[5];
+    this->accel_x = (out[0] << 8) | out[1];
+    this->accel_y = (out[2] << 8) | out[3];
+    this->accel_z = (out[4] << 8) | out[5];
 
-    this->temp = (out[6] << 8) + out[7];
+    this->temp = (out[6] << 8) | out[7];
 
-    this->gyro_x = (out[8] << 8) + out[9];
-    this->gyro_y = (out[10] << 8) + out[11];
-    this->gyro_z = (out[12] << 8) + out[13];
+    this->gyro_x = (out[8] << 8) | out[9];
+    this->gyro_y = (out[10] << 8) | out[11];
+    this->gyro_z = (out[12] << 8) | out[13];
 }
 
 int16_t Sensors::getAccelX()
@@ -106,7 +144,7 @@
 }
 int16_t Sensors::getGyroX()
 {
-    return this->accel_x;
+    return this->gyro_x;
 }
 int16_t Sensors::getGyroY()
 {
@@ -132,26 +170,10 @@
     i2c.write(MPU6500,cmd,2);
 }
 
-void Sensors::setDMP(int8_t th)
-{
-    writeRegister(0x6A, ( readRegister(0x6A) | (1 << 3))); //reset DMP
-    writeRegister(0x6A, ( readRegister(0x6A) | (1 << 7))); //set DMP
-    writeRegister(0x38, ( readRegister(0x38) | (1 << 6))); //enable motion interrupt
-    writeRegister(0x38, ( readRegister(0x38) | (1 << 1))); //enable motion interrupt
-    writeRegister(0x1F, th); //set threshold
-}
-
-
-bool Sensors::checkDMP()
-{
-    
-    if (((readRegister(0x3A) >> 6) & 1) == 1) {
-        return true;
-    }  else {
-        return false;
-    }
-}
 
 
 
 
+
+
+