Revision 4:6d1118089a36, committed 2015-04-10

Comitter:

kenjiArai

Date:

Fri Apr 10 11:19:49 2015 +0000

Parent:

3:f5b5c4d795ce

Child:

5:9594519c9462

Commit message:

Added manual calibration routine

Changed in this revision

--- a/BNO055_fusion.lib	Wed Apr 08 11:30:09 2015 +0000
+++ b/BNO055_fusion.lib	Fri Apr 10 11:19:49 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/kenjiArai/code/BNO055_fusion/#0f225b686cd5
+http://developer.mbed.org/users/kenjiArai/code/BNO055_fusion/#0ad6f85b178f

--- a/main.cpp	Wed Apr 08 11:30:09 2015 +0000
+++ b/main.cpp	Fri Apr 10 11:19:49 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      8th, 2015
+ *      Revised: April     10th, 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
@@ -23,9 +23,6 @@
 //  Definition ------------------------------------------------------------------------------------
 #define NUM_LOOP    100
 
-#define FORMAT_0    0    
-#define FORMAT_1    1
-
 //  Object ----------------------------------------------------------------------------------------
 Serial pc(USBTX,USBRX);
 #if defined(TARGET_LPC1114)
@@ -64,8 +61,127 @@
 //-------------------------------------------------------------------------------------------------
 //  Control Program
 //-------------------------------------------------------------------------------------------------
-#if FORMAT_0
+// Calibration
+//  Please refer BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data
+void bno055_calbration(void){
+    uint8_t d;
 
+    pc.printf("------ 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
+    pc.printf("Step1) Please wait few seconds\r\n");
+    t.start();
+    while (t.read() < 10){
+        d = imu.read_calib_status();
+        pc.printf("Calb dat = 0x%x target  = 0x30(at least)\r\n", d);
+        if ((d & 0x30) == 0x30){
+            break;
+        }
+        wait(1.0);
+    }
+    pc.printf("-> Step1) is done\r\n\r\n");
+    //---------- Magnetometer Caliblation ---------------------------------------------------------
+    // (a) Make some random movements (for example: writing the number ‘8’ on air) until the
+    //     CALIB_STAT register indicates fully calibrated.
+    // (b) It takes more calibration movements to get the magnetometer calibrated than in the
+    //     NDOF mode.
+    pc.printf("Step2) random moving (try to change the BNO055 axis)\r\n");
+    t.start();
+    while (t.read() < 30){
+        d = imu.read_calib_status();
+        pc.printf("Calb dat = 0x%x target  = 0x33(at least)\r\n", d);
+        if ((d & 0x03) == 0x03){
+            break;
+        }
+        wait(1.0);   
+    }
+    pc.printf("-> Step2) is done\r\n\r\n");
+    //---------- Magnetometer Caliblation ---------------------------------------------------------
+    // a) Place the device in 6 different stable positions for a period of few seconds
+    //    to allow the accelerometer to calibrate.
+    // b) Make sure that there is slow movement between 2 stable positions
+    //    The 6 stable positions could be in any direction, but make sure that the device is
+    //    lying at least once perpendicular to the x, y and z axis.
+    pc.printf("Step3) Change rotation each X,Y,Z axis KEEP SLOWLY!!");
+    pc.printf(" Each 90deg stay a 5 sec and set at least 6 position.\r\n");
+    pc.printf(" e.g. (1)ACC:X0,Y0,Z-9,(2)ACC:X9,Y0,Z0,(3)ACC:X0,Y0,Z9,");
+    pc.printf("(4)ACC:X-9,Y0,Z0,(5)ACC:X0,Y-9,Z0,(6)ACC:X0,Y9,Z0,\r\n");
+    pc.printf(" If you will give up, hit any key.\r\n", d);
+    t.stop();
+    while (true){
+        d = imu.read_calib_status();
+        imu.get_gravity(&gravity);
+        pc.printf("Calb dat = 0x%x target  = 0xff ACC:X %3.0f, Y %3.0f, Z %3.0f\r\n",
+                   d, gravity.x, gravity.y, gravity.z);
+        if (d == 0xff){     break;}
+        if (pc.readable()){ break;}
+        wait(1.0);
+    }
+    if (imu.read_calib_status() == 0xff){
+        pc.printf("-> All of Calibration steps are done successfully!\r\n\r\n");
+    } else {
+        pc.printf("-> Calibration steps are suspended!\r\n\r\n");
+    }
+    t.stop();
+}
+
+int main(){
+    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");
+    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 Reset\r\n");
+            pwr_onoff = 1;  // Power off
+            wait(0.1);
+            pwr_onoff = 0;  // Power on
+            wait(0.02);
+        } while(imu.reset());
+    }
+    pc.printf("Bosch BNO055 is available now!!\r\n");
+    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 ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x, ",
+        bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id);
+    pc.printf("SW REV:0x%04x, BL REV:0x%02x\r\n",
+        bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
+    pc.printf("If you would like to calibrate the BNO055, please hit 'y' (No: any other key)\r\n");
+    c = pc.getc();
+    if (c == 'y'){
+        bno055_calbration();
+    }
+    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());
+    }
+}
+
+
+// Diffrent output format as for your reference
+#if 0
 int main() {
     uint8_t i;
 
@@ -126,53 +242,4 @@
         }
     }
 }
-
-#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