Diff: Sensors.cpp

Revision:

5:46947b447701

Parent:

4:2a5a08b14539

Child:

6:75263c93daf7

--- a/Sensors.cpp	Wed Jun 03 15:10:45 2015 +0000
+++ b/Sensors.cpp	Sun Jun 07 14:11:26 2015 +0000
@@ -9,7 +9,7 @@
 
 Sensors::Sensors()
 {
-    i2c.frequency(300000);
+    i2c.frequency(250000);
 
     this->angle_h = 0;
     this->angle_l = 0;
@@ -29,8 +29,9 @@
     this->gyro_z_l = 0;
 
 
-    setupIMU();
+    //setupIMU();
     setupAngle();
+    //disableIMU();
 }
 
 
@@ -73,15 +74,15 @@
 void Sensors::disableIMU(){
     writeRegister(0x6B, (readRegister(0x6B) | 1 << 6));
     }
+    
+int8_t Sensors::getFs(){
+    return 8000 / (readRegister(0x19) + 1);
+    }
 
 void Sensors::setupIMU()
 {
     //Sets sample rate to 8000/1+79 = 100Hz
     writeRegister(0x19,0x4f); //4f = 100 hz, 9f = 50 hz
-    
-    wait(0.001);
-    
-    writeRegister(0x19,0x4f); //4f = 100 hz, 9f = 50 hz
     //Disable FSync, 256 DLPF
     writeRegister(0x1A,0x00);
     
@@ -137,18 +138,6 @@
     cmd[0] = 0x3B;
     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->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->accel_x_h = out[0];
     this->accel_x_l = out[1];
@@ -156,7 +145,10 @@
     this->accel_y_l = out[3];
     this->accel_z_h = out[4];
     this->accel_z_l = out[5];
-
+        
+    int16_t temp16 = ((out[6] << 8) | out[7]);
+    
+    this->temp = temp16/340 + 36.53;
 
     this->gyro_x_h = out[8];
     this->gyro_x_l = out[9];
@@ -220,7 +212,10 @@
 int16_t Sensors::getGyroX(){
     return ((getGyroXH() << 8) + getGyroXL());
     }
-
+    
+float Sensors::getTemp(){
+    return this->temp;
+    }
 
 int8_t Sensors::readRegister(int8_t addr)
 {