Diff: main.cpp

Revision:

6:0590c7ff8c34

Parent:

5:9594519c9462

Child:

7:d4b5e83c7947

--- a/main.cpp	Thu Apr 16 11:25:47 2015 +0000
+++ b/main.cpp	Sun Jan 24 17:04:40 2016 +0000
@@ -25,25 +25,11 @@
 
 //  Object ----------------------------------------------------------------------------------------
 Serial pc(USBTX,USBRX);
-#if defined(TARGET_LPC1114)
-DigitalOut pwr_onoff(dp17);
-I2C    i2c(dp5, dp27);   // SDA, SCL
-BNO055 imu(i2c, dp18);  // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default
-#elif defined(TARGET_LPC1768)
+
 DigitalOut pwr_onoff(p30);
 I2C    i2c(p28, p27); // SDA, SCL
 BNO055 imu(i2c, p29);  // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default
-#elif defined(TARGET_STM32L152RE) || defined(TARGET_STM32F401RE) || defined(TARGET_STM32F411RE)
-DigitalOut pwr_onoff(PB_10);
-I2C    i2c(PB_9, PB_8); // SDA, SCL
-BNO055 imu(i2c, PA_8);  // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default
-#elif defined(TARGET_RZ_A1H)
-DigitalOut pwr_onoff(P8_11);
-I2C    i2c(P1_3, P1_2); // SDA, SCL
-BNO055 imu(i2c, P8_13);  // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default
-#else
-#error "Not cheched yet"
-#endif
+
 Timer t;
 
 //  RAM -------------------------------------------------------------------------------------------
@@ -66,7 +52,7 @@
 void bno055_calbration(void){
     uint8_t d;
 
-    pc.printf("------ Enter BNO055 Manual Calibration Mode ------\r\n");
+    pc.printf("calibrating:, ------ Enter BNO055 Manual Calibration Mode ------\r\n");
     //---------- Gyroscope Caliblation ------------------------------------------------------------
     // (a) Place the device in a single stable position for a period of few seconds to allow the
     //     gyroscope to calibrate
@@ -74,7 +60,7 @@
     t.start();
     while (t.read() < 10){
         d = imu.read_calib_status();
-        pc.printf("Calb dat = 0x%x target  = 0x30(at least)\r\n", d);
+        pc.printf("calibrating = 0x%x target  = 0x30(at least)\r\n", d);
         if ((d & 0x30) == 0x30){
             break;
         }
@@ -90,7 +76,7 @@
     t.start();
     while (t.read() < 30){
         d = imu.read_calib_status();
-        pc.printf("Calb dat = 0x%x target  = 0x33(at least)\r\n", d);
+        pc.printf("calibrating, = 0x%x target  = 0x33(at least)\r\n", d);
         if ((d & 0x03) == 0x03){
             break;
         }
@@ -112,7 +98,7 @@
     while (true){
         d = imu.read_calib_status();
         imu.get_gravity(&gravity);
-        pc.printf("Calb dat = 0x%x target  = 0xff ACC:X %4.1f, Y %4.1f, Z %4.1f\r\n",
+        pc.printf("calibrating = 0x%x target  = 0xff ACC:X %4.1f, Y %4.1f, Z %4.1f\r\n",
                    d, gravity.x, gravity.y, gravity.z);
         if (d == 0xff){     break;}
         if (pc.readable()){ break;}
@@ -125,17 +111,19 @@
     }
     t.stop();
 }
-
+//--------------------------------------------------------------------------------
 int main(){
+    pc.baud(9600);
+    pc.printf("notstreaming: BNO055 Hello World\r\n\r\n");
     imu.set_mounting_position(MT_P6);
     pwr_onoff = 0;
-    pc.printf("\r\n\r\nIf pc terminal soft is ready, please hit any key!\r\n");
+    pc.printf(",\r\n\r\nIf pc terminal soft is ready, please hit any key!\r\n");
     char c = pc.getc();
-    pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
+    pc.printf(",Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
     // Is BNO055 avairable?
     if (imu.chip_ready() == 0){
         do {
-            pc.printf("Bosch BNO055 is NOT avirable!!\r\n Reset\r\n");
+            pc.printf(",Bosch BNO055 is NOT avirable!!\r\n Reset\r\n");
             pwr_onoff = 1;  // Power off
             wait(0.1);
             pwr_onoff = 0;  // Power on
@@ -158,88 +146,56 @@
     pc.printf("[E]:Euler Angles[deg],[Q]:Quaternion[],[L]:Linear accel[m/s*s],");
     pc.printf("[G]:Gravity vector[m/s*s],[T]:Chip temperature,Acc,Gyr[degC],[S]:Status,[M]:time[mS]\r\n");
     t.start();
-    while(1) {
+    
+    //Set Register into Config Mode
+    //imu.write_reg0(0x3D, 0x00); 
+    //Set register unit to Radians
+    imu.write_reg0(0x3B, 0x02);
+    //Change back into normal operation mode
+    //imu.write_reg0(0x3D, 0x0C);
+    
+    while(1) {       
         imu.get_Euler_Angles(&euler_angles);
-        pc.printf("[E],Y,%+6.1f,R,%+6.1f,P,%+6.1f,",
-                   euler_angles.h, euler_angles.r, euler_angles.p);
-        imu.get_quaternion(&quaternion);
-        pc.printf("[Q],W,%d,X,%d,Y,%d,Z,%d,",
-                   quaternion.w, quaternion.x, quaternion.y, quaternion.z);
+        pc.printf("Streaming:,Orientation:,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
+                euler_angles.h, euler_angles.r, euler_angles.p);
         imu.get_linear_accel(&linear_acc);
-        pc.printf("[L],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
-                   linear_acc.x, linear_acc.y, linear_acc.z);
+        pc.printf("Acc:,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
+                linear_acc.x, linear_acc.y, linear_acc.z);
+                
         imu.get_gravity(&gravity);
-        pc.printf("[G],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
+        pc.printf("Gravity,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
                    gravity.x, gravity.y, gravity.z);
-        imu.get_chip_temperature(&chip_temp);
-        pc.printf("[T],%+d,%+d,",
-                   chip_temp.acc_chip, chip_temp.gyr_chip);
-        pc.printf("[S],0x%x,[M],%d\r\n",
-                   imu.read_calib_status(), t.read_ms());
+                
+        pc.printf("Time:,%d,\n",t.read_ms());
+        
+        
+                   
+//        c = pc.getc();
+//        if (c == 'c'){
+//         bno055_calbration();
+//        }      
+//        if (c == 'r'){
+//         imu.reset();
+//        }                             
     }
 }
 
-
-// Diffrent output format as for your reference
-#if 0
-int main() {
-    uint8_t i;
+//        imu.get_Euler_Angles(&euler_angles);
+//        pc.printf("[E],Y,%+6.1f,R,%+6.1f,P,%+6.1f,",
+//                   euler_angles.h, euler_angles.r, euler_angles.p);
+//        imu.get_quaternion(&quaternion);
+//        pc.printf("[Q],W,%d,X,%d,Y,%d,Z,%d,",
+//                   quaternion.w, quaternion.x, quaternion.y, quaternion.z);
+//        imu.get_linear_accel(&linear_acc);
+//        pc.printf("[L],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
+//                   linear_acc.x, linear_acc.y, linear_acc.z);
+//        imu.get_gravity(&gravity);
+//        pc.printf("[G],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
+//                   gravity.x, gravity.y, gravity.z);
+//        imu.get_chip_temperature(&chip_temp);
+//        pc.printf("[T],%+d,%+d,",
+//                   chip_temp.acc_chip, chip_temp.gyr_chip);
+//        pc.printf("[S],0x%x,[M],%d\r\n",
+//                   imu.read_calib_status(), t.read_ms());
 
-    pwr_onoff = 0;
-    pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
-    // Is BNO055 avairable?
-    if (imu.chip_ready() == 0){
-        do {
-            pc.printf("Bosch BNO055 is NOT avirable!!\r\n");
-            pwr_onoff = 1;  // Power off
-            wait(0.1);
-            pwr_onoff = 0;  // Power on
-            wait(0.02);
-        } while(imu.reset());
-    }
-    imu.set_mounting_position(MT_P6);
-    pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n",
-               imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN));    
-    imu.read_id_inf(&bno055_id_inf);
-    pc.printf("CHIP:0x%02x, ACC:0x%02x, MAG:0x%02x, GYR:0x%02x, , SW:0x%04x, , BL:0x%02x\r\n",
-               bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id,
-               bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
-    while(1) {
-        pc.printf("Euler Angles data\r\n");
-        for (i = 0; i < NUM_LOOP; i++){
-            imu.get_Euler_Angles(&euler_angles);
-            pc.printf("Heading:%+6.1f [deg], Roll:%+6.1f [deg], Pich:%+6.1f [deg], #%02d\r\n",
-                       euler_angles.h, euler_angles.r, euler_angles.p, i);
-            wait(0.5);
-        }
-        pc.printf("Quaternion data\r\n");
-        for (i = 0; i < NUM_LOOP; i++){
-            imu.get_quaternion(&quaternion);
-            pc.printf("W:%d, X:%d, Y:%d, Z:%d, #%02d\r\n",
-                       quaternion.w, quaternion.x, quaternion.y, quaternion.z, i);
-            wait(0.5);
-        }
-        pc.printf("Linear accel data\r\n");
-        for (i = 0; i < NUM_LOOP; i++){
-            imu.get_linear_accel(&linear_acc);
-            pc.printf("X:%+6.1f [m/s*s], Y:%+6.1f [m/s*s], Z:%+6.1f [m/s*s], #%02d\r\n",
-                       linear_acc.x, linear_acc.y, linear_acc.z, i);
-            wait(0.5);
-        }
-        pc.printf("Gravity vector data\r\n");
-        for (i = 0; i < NUM_LOOP; i++){
-            imu.get_gravity(&gravity);
-            pc.printf("X:%+6.1f [m/s*s], Y:%+6.1f [m/s*s], Z:%+6.1f [m/s*s], #%02d\r\n",
-                       gravity.x, gravity.y, gravity.z, i);
-            wait(0.5);
-        }
-        pc.printf("Chip temperature data\r\n");
-        for (i = 0; i < (NUM_LOOP / 4); i++){
-            imu.get_chip_temperature(&chip_temp);
-            pc.printf("Acc chip:%+d [degC], Gyr chip:%+d [degC], #%02d\r\n",
-                       chip_temp.acc_chip, chip_temp.gyr_chip, i);
-            wait(0.5);
-        }
-    }
-}
-#endif
+//        pc.printf("Words\r\n");
\ No newline at end of file