Diff: inv_mpu.cpp

Revision:

1:dbbd3dcd61c9

Parent:

0:4dd021344a6b

Child:

3:8ac73f9099ab

--- a/inv_mpu.cpp	Tue Jun 03 07:54:12 2014 +0000
+++ b/inv_mpu.cpp	Sat Jun 07 21:05:23 2014 +0000
@@ -33,7 +33,6 @@
 #else
     #define PRINTF(...)
     #define BREAK 
-    #define PRINTF
     #define PRINTLN(x)
 #endif
 
@@ -747,21 +746,21 @@
 {
     unsigned char data[6];
 
-//    PRINTLN("mpu_init");
+    PRINTLN("mpu_init");
     /* Reset device. */
     data[0] = BIT_RESET;
     if (i2Cdev.writeBytes(st.hw->addr, st.reg->pwr_mgmt_1, 1, data) )
     {
         return -1;
     }
-//    PRINTLN("Device Reset");
+    PRINTLN("Device Reset");
     wait_ms(100);
 
     /* Wake up chip. */
     data[0] = 0x00;
     if (i2Cdev.writeBytes(st.hw->addr, st.reg->pwr_mgmt_1, 1, data))
         return -1;
-//    PRINTLN("Wake up Chip");
+    PRINTLN("Wake up Chip");
 
    st.chip_cfg.accel_half = 0;
 
@@ -799,19 +798,19 @@
 
     if (mpu_set_gyro_fsr(2000))
         return -1;
-//    PRINTLN("mpu_set_gyro_fsr");
+    PRINTLN("mpu_set_gyro_fsr");
     if (mpu_set_accel_fsr(2))
         return -1;
-//    PRINTLN("mpu_set_accel_fsr");
+    PRINTLN("mpu_set_accel_fsr");
     if (mpu_set_lpf(42))
         return -1;
-//    PRINTLN("mpu_set_lpf");
+    PRINTLN("mpu_set_lpf");
     if (mpu_set_sample_rate(50))
         return -1;
-//    PRINTLN("mpu_set_sample_rate");
+    PRINTLN("mpu_set_sample_rate");
     if (mpu_configure_fifo(0))
         return -1;
-//    PRINTLN("mpu_configure_fifo");
+    PRINTLN("mpu_configure_fifo");
 
 //    if (int_param)
 //        reg_int_cb(int_param);
@@ -825,6 +824,7 @@
     if (mpu_set_bypass(0))
         return -1;
 #endif
+    PRINTLN("mpu_set_compass_sample_rate");
 
     mpu_set_sensors(0);
     return 0;
@@ -1758,7 +1758,7 @@
 
     st.chip_cfg.sensors = sensors;
     st.chip_cfg.lp_accel_mode = 0;
-    wait_ms(50);
+    wait_ms(20);
     return 0;
 }