Diff: RTOS-Setup/src/setup.cpp

Revision:

48:9dbdc4144f00

Parent:

39:02782ad251db

Child:

49:c882f9135033

diff -r b11655031d24 -r 9dbdc4144f00 RTOS-Setup/src/setup.cpp
--- a/RTOS-Setup/src/setup.cpp	Sat May 17 11:57:09 2014 +0000
+++ b/RTOS-Setup/src/setup.cpp	Mon May 19 13:21:17 2014 +0000
@@ -8,13 +8,15 @@
 
 Serial BT(p28, p27);
 DigitalOut BT_CMD(p29);
-MPU6050 imu(p9, p10);           // MPU6050-Tilty SDA, SCL (18, 19)
+//MPU6050 imu(p9, p10);           
+MPU6050_NB imu_nb(p9, p10); 
 MPL3115A2 altimeter(p9, p10);
+AnalogIn voltageSense(p20);
 #ifdef ENABLE_COMPASS
 HMC5883L compass(p9, p10);
 #endif
 
-float KP_YAW_RATE           =   P_Y_RATE;                
+float KP_YAW_RATE           =   P_Y_RATE;
 float KP_PITCH_RATE         =   P_P_RATE;
 float KP_ROLL_RATE          =   P_R_RATE;
 
@@ -43,38 +45,38 @@
 // === MAIN SETUP ROUTINE ===
 // ==========================
 bool setupALLdevices(void)
-{
+{   
     bool error = false;
     box_demo = false;
     mode = ATTITUDE;
 
     if (!setup_ESC()) {
-        imu.debugSerial.printf("ESC FAILED!!!\n");
+        //imu.debugSerial.printf("ESC FAILED!!!\n");
         error = true;
     }
 
-    if (setup_bt())
-        imu.debugSerial.printf("BT established!\n");
+    if (setup_bt()) {}
+        //imu.debugSerial.printf("BT established!\n");
     else error = true;
 
-    if (setup_PID())
-        imu.debugSerial.printf("PID established!\n");
+    if (setup_PID()) {}
+        //imu.debugSerial.printf("PID established!\n");
     else error = true;
 
-    if (setup_mpu6050())
-        imu.debugSerial.printf("MPU6050 established!\n");
+    if (setup_mpu6050()) {}
+        //imu.debugSerial.printf("MPU6050 established!\n");
     else error = true;
 
-    if (setup_altimeter())
-        imu.debugSerial.printf("Altimeter established!\n");
-    else {
-        error = true;
-        imu.debugSerial.printf("ALTIMETER FAILED\n");
-    }
+    if (setup_altimeter()) {}
+        //imu.debugSerial.printf("Altimeter established!\n");
+     else {
+         error = true;
+        //imu.debugSerial.printf("ALTIMETER FAILED\n");
+     }
 
 #ifdef ENABLE_COMPASS
     if (setup_compass())
-        imu.debugSerial.printf("Compass established!\n");
+        //imu.debugSerial.printf("Compass established!\n");
     else error = true;
 #endif
 
@@ -94,7 +96,7 @@
 
     for (int i = 0; i < 4; i++)
         ESC[i].pulsewidth_us(1000);
-    
+
     for (int i = 0; i < 4; i++)
         ESCpower[i] = 990;
 
@@ -143,7 +145,7 @@
     rollPIDrate.setOutputLimits(-200.0, 200.0);
     rollPIDrate.setBias(0.0);
     rollPIDrate.setMode(AUTO_MODE);
-    
+
     pitchPIDattitude.reset();
     rollPIDattitude.reset();
     yawPIDrate.reset();
@@ -165,6 +167,8 @@
 
 bool setup_mpu6050(void)
 {
+    MPU6050 imu(p9, p10); 
+    
     imu.reset();
     imu.debugSerial.baud(115200);
     wait_ms(5);
@@ -172,12 +176,11 @@
     imu.initialize();
     imu_available = imu.testConnection();
 
-    imu.debugSerial.printf("IMU status...\t\t\t");
+    imu.debugSerial.printf("imu status...\t\t\t");
     imu_available ? imu.debugSerial.printf("OK!\n") : imu.debugSerial.printf("NOT OK!\n");
 
     if (imu_available) {
         devStatus = imu.dmpInitialize();
-
         // supply your own gyro offsets here, scaled for min sensitivity
         //imu.setXGyroOffset(55);
         //imu.setYGyroOffset(3);
@@ -207,7 +210,7 @@
         return false;
     }
 
-    imu.debugSerial.printf("IMU setup routine done!");
+    imu.debugSerial.printf("imu setup routine done!");
 
     return dmpReady;
 }