Diff: main.cpp

Revision:

3:f5b5c4d795ce

Parent:

2:cf77282aea7b

Child:

4:6d1118089a36

--- a/main.cpp	Tue Apr 07 12:11:39 2015 +0000
+++ b/main.cpp	Wed Apr 08 11:30:09 2015 +0000
@@ -7,7 +7,7 @@
  *  http://www.page.sannet.ne.jp/kenjia/index.html
  *  http://mbed.org/users/kenjiArai/
  *      Created: March     30th, 2015
- *      Revised: April      7th, 2015
+ *      Revised: April      8th, 2015
  *
  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
  * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
@@ -21,7 +21,10 @@
 #include    "BNO055.h"
 
 //  Definition ------------------------------------------------------------------------------------
-#define NUM_LOOP 100
+#define NUM_LOOP    100
+
+#define FORMAT_0    0    
+#define FORMAT_1    1
 
 //  Object ----------------------------------------------------------------------------------------
 Serial pc(USBTX,USBRX);
@@ -44,6 +47,7 @@
 #else
 #error "Not cheched yet"
 #endif
+Timer t;
 
 //  RAM -------------------------------------------------------------------------------------------
 BNO055_ID_INF_TypeDef       bno055_id_inf;
@@ -59,7 +63,9 @@
 
 //-------------------------------------------------------------------------------------------------
 //  Control Program
-//------------------------------------------------------------------------------------------------- 
+//-------------------------------------------------------------------------------------------------
+#if FORMAT_0
+
 int main() {
     uint8_t i;
 
@@ -120,3 +126,53 @@
         }
     }
 }
+
+#elif FORMAT_1
+
+int main() {
+    pwr_onoff = 0;
+    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");
+    // 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);
+    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) {
+        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());
+    }
+}
+
+#endif