Diff: main.cpp

Revision:

2:cf77282aea7b

Parent:

0:31451519d283

Child:

3:f5b5c4d795ce

--- a/main.cpp	Sun Apr 05 04:18:03 2015 +0000
+++ b/main.cpp	Tue Apr 07 12:11:39 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      5th, 2015
+ *      Revised: April      7th, 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,11 +21,29 @@
 #include    "BNO055.h"
 
 //  Definition ------------------------------------------------------------------------------------
+#define NUM_LOOP 100
 
 //  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);        // addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default
+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
 
 //  RAM -------------------------------------------------------------------------------------------
 BNO055_ID_INF_TypeDef       bno055_id_inf;
@@ -45,10 +63,17 @@
 int main() {
     uint8_t i;
 
+    pwr_onoff = 0;
     pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
     // Is BNO055 avairable?
     if (imu.chip_ready() == 0){
-        pc.printf("Bosch BNO055 is NOT avirable!!\r\n");
+        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",
@@ -59,35 +84,35 @@
                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 < 20; i++){
+        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 < 20; i++){
+        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 < 20; i++){
+        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 < 20; i++){
+        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 < 20; i++){
+        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);