Diff: Sensor.cpp

Revision:

4:f817736140b6

Parent:

3:e658293d4b83

--- a/Sensor.cpp	Wed Oct 15 04:59:43 2014 +0000
+++ b/Sensor.cpp	Sun Nov 02 19:17:54 2014 +0000
@@ -1,6 +1,7 @@
 
 #include "Sensor.h"
-#ifdef DEBUG_S
+//#define _DEBUG_S
+#ifdef _DEBUG_S
     #include "mbed.h"
     Serial p(USBTX, USBRX);
     Timer td;
@@ -50,9 +51,10 @@
 {
     int result = 0;
     result = mpu_initialize(); 
-    if (result) return result;
+    if (result != 0) return result;
+    wait_ms(20);
     result = dmp_initialize();
-    if (result) return result;
+    if (result != 0) return result;
     run_self_test();
     return 0;  
 }
@@ -61,11 +63,9 @@
 {    
     unsigned char accel_fsr;
     unsigned short gyro_rate, gyro_fsr;
-    long accel_bias[3], gyro_bias[3];
+//    long accel_bias[3], gyro_bias[3];
     if( !dmp.testConnection() )
         return -1;
-    if( dmp.mpu_init() )
-        return -2;
     /* Get/set hardware configuration. Start gyro. */
     /* Wake up all sensors. */
     if( dmp.mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) )
@@ -80,7 +80,8 @@
     dmp.mpu_get_gyro_fsr(&gyro_fsr);
     dmp.mpu_get_accel_fsr(&accel_fsr);
     
-    if( !(gyro_rate == SAMPLE_RATE && gyro_fsr == 2000 && accel_fsr == 2) )
+//    if( !(gyro_rate == SAMPLE_RATE && gyro_fsr == 2000 && accel_fsr == 2) )
+    if( !(gyro_rate == SAMPLE_RATE) )
         return -6;
 
     return 0;
@@ -99,8 +100,8 @@
 //    unsigned short mask = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_SEND_CAL_GYRO;
     if( dmp.dmp_enable_feature(mask) )
         return -10;
-    if( dmp.mpu_set_sample_rate(200) )
-        return -11;
+//    if( dmp.mpu_set_sample_rate(200) )
+//        return -11;
     if( dmp.dmp_set_fifo_rate(100) )
         return -12; 
     if( dmp.mpu_set_dmp_state(1) )
@@ -116,7 +117,7 @@
 }
 
 
-void Sensor::run_self_test()
+int Sensor::run_self_test()
 {
     int result;
     long gyro_cal[3], accel_cal[3];
@@ -135,7 +136,9 @@
         }
         dmp.mpu_set_gyro_bias_reg(gyro_cal);
         dmp.mpu_set_accel_bias_6050_reg(accel_cal);
+        return 0;
     }
+    return -1;
 }
 
 int Sensor::updateAngles()