Revision 7:b48d96169302, committed 2020-08-05

Comitter:

kenjiArai

Date:

Wed Aug 05 05:28:13 2020 +0000

Parent:

6:07d01bf36ad0

Commit message:

run on mbed-os6.2.0

Changed in this revision

diff -r 07d01bf36ad0 -r b48d96169302 BNO055.cpp
--- a/BNO055.cpp	Wed Aug 23 09:44:43 2017 +0000
+++ b/BNO055.cpp	Wed Aug 05 05:28:13 2020 +0000
@@ -3,26 +3,22 @@
  *  BNO055 Intelligent 9-axis absolute orientation sensor
  *  by Bosch Sensortec
  *
- * Copyright (c) 2015,'17 Kenji Arai / JH1PJL
- *  http://www.page.sannet.ne.jp/kenjia/index.html
- *  http://mbed.org/users/kenjiArai/
+ * Copyright (c) 2015,'17,'20 Kenji Arai / JH1PJL
+ *  http://www7b.biglobe.ne.jp/~kenjia/
+ *  https://os.mbed.com/users/kenjiArai/
  *      Created: March     30th, 2015
- *      Revised: August    23rd, 2017
+ *      Revised: August     5th, 2020
  */
 
 #include "mbed.h"
 #include "BNO055.h"
 
-
-#if MBED_MAJOR_VERSION == 2
-#define WAIT_MS(x)       wait_ms(x)
-#elif  MBED_MAJOR_VERSION == 5
-#define WAIT_MS(x)       Thread::wait(x)
-#else
-#error "Running on Unknown OS"
-#endif
-
-BNO055::BNO055 (PinName p_sda, PinName p_scl, PinName p_reset, uint8_t addr, uint8_t mode):
+BNO055::BNO055 (PinName p_sda,
+                PinName p_scl,
+                PinName p_reset,
+                uint8_t addr,
+                uint8_t mode
+               ):
     _i2c_p(new I2C(p_sda, p_scl)), _i2c(*_i2c_p), _res(p_reset)
 {
     chip_addr = addr;
@@ -54,7 +50,7 @@
     initialize ();
 }
 
-/////////////// Read data & normalize /////////////////////
+/////////////// Read data & normalize //////////////////////////////////////////
 void BNO055::get_Euler_Angles(BNO055_EULER_TypeDef *el)
 {
     uint8_t deg_or_rad;
@@ -176,7 +172,7 @@
     dt[0] = BNO055_TEMP_SOURCE;
     dt[1] = 0;
     _i2c.write(chip_addr, dt, 2, false);
-    WAIT_MS(1); // Do I need to wait?
+    ThisThread::sleep_for(1ms); // Do I need to wait?
     dt[0] = BNO055_TEMP;
     _i2c.write(chip_addr, dt, 1, true);
     _i2c.read(chip_addr, dt, 1, false);
@@ -188,7 +184,7 @@
     dt[0] = BNO055_TEMP_SOURCE;
     dt[1] = 1;
     _i2c.write(chip_addr, dt, 2, false);
-    WAIT_MS(1); // Do I need to wait?
+    ThisThread::sleep_for(1ms); // Do I need to wait?
     dt[0] = BNO055_TEMP;
     _i2c.write(chip_addr, dt, 1, true);
     _i2c.read(chip_addr, dt, 1, false);
@@ -199,10 +195,10 @@
     }
 }
 
-/////////////// Initialize ////////////////////////////////
+/////////////// Initialize /////////////////////////////////////////////////////
 void BNO055::initialize (void)
 {
-#if defined(TARGET_STM32L152RE)
+#if defined(TARGET_NUCLEO_L152RE)
     _i2c.frequency(100000);
 #else
     _i2c.frequency(400000);
@@ -223,13 +219,14 @@
 {
     select_page(0);
     dt[0] = BNO055_UNIT_SEL;
-    dt[1] = UNIT_ORI_WIN + UNIT_ACC_MSS + UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C;
+    dt[1] = UNIT_ORI_WIN + UNIT_ACC_MSS +
+            UNIT_GYR_DPS + UNIT_EULER_DEG + UNIT_TEMP_C;
     _i2c.write(chip_addr, dt, 2, false);
 }
 
 uint8_t BNO055::select_page(uint8_t page)
 {
-    if (page != page_flag){
+    if (page != page_flag) {
         dt[0] = BNO055_PAGE_ID;
         if (page == 1) {
             dt[1] = 1;  // select page 1
@@ -247,11 +244,11 @@
 
 uint8_t BNO055::reset(void)
 {
-     _res = 0;
-     WAIT_MS(1);   // Reset 1mS
-     _res = 1;
-     WAIT_MS(700); // Need to wait at least 650mS
-#if defined(TARGET_STM32L152RE)
+    _res = 0;
+    ThisThread::sleep_for(1ms);   // Reset 1mS
+    _res = 1;
+    ThisThread::sleep_for(700ms); // Need to wait at least 650mS
+#if defined(TARGET_NUCLEO_L152RE)
     _i2c.frequency(400000);
 #else
     _i2c.frequency(400000);
@@ -260,7 +257,7 @@
     page_flag = 0xff;
     select_page(0);
     check_id();
-    if (chip_id != I_AM_BNO055_CHIP){
+    if (chip_id != I_AM_BNO055_CHIP) {
         return 1;
     } else {
         initialize();
@@ -268,14 +265,14 @@
     }
 }
 
-////// Set initialize data to related registers ///////////
+////// Set initialize data to related registers ////////////////////////////////
 void BNO055::set_initial_dt_to_regs(void)
 {
     // select_page(0);
     // current setting is only used default values
 }
 
-/////////////// Check Who am I? ///////////////////////////
+/////////////// Check Who am I? ////////////////////////////////////////////////
 void BNO055::check_id(void)
 {
     select_page(0);
@@ -315,7 +312,7 @@
     id->sw_rev_id = sw_rev_id;
 }
 
-/////////////// Check chip ready or not  //////////////////
+/////////////// Check chip ready or not  ///////////////////////////////////////
 uint8_t BNO055::chip_ready(void)
 {
     if (ready_flag == 0x0f) {
@@ -324,7 +321,7 @@
     return 0;
 }
 
-/////////////// Read Calibration status  //////////////////
+/////////////// Read Calibration status  ///////////////////////////////////////
 uint8_t BNO055::read_calib_status(void)
 {
     select_page(0);
@@ -334,7 +331,7 @@
     return dt[0];
 }
 
-/////////////// Change Fusion mode  ///////////////////////
+/////////////// Change Fusion mode  ////////////////////////////////////////////
 void BNO055::change_fusion_mode(uint8_t mode)
 {
     uint8_t current_mode;
@@ -346,23 +343,24 @@
             dt[0] = BNO055_OPR_MODE;
             dt[1] = mode;
             _i2c.write(chip_addr, dt, 2, false);
-            WAIT_MS(19);    // wait 19mS
+            ThisThread::sleep_for(19ms);    // wait 19mS
             break;
         case MODE_IMU:
         case MODE_COMPASS:
         case MODE_M4G:
         case MODE_NDOF_FMC_OFF:
         case MODE_NDOF:
-            if (current_mode != CONFIGMODE) {   // Can we change the mode directry?
+            // Can we change the mode directry?
+            if (current_mode != CONFIGMODE) {
                 dt[0] = BNO055_OPR_MODE;
                 dt[1] = CONFIGMODE;
                 _i2c.write(chip_addr, dt, 2, false);
-                WAIT_MS(19);    // wait 19mS
+                ThisThread::sleep_for(19ms);    // wait 19mS
             }
             dt[0] = BNO055_OPR_MODE;
             dt[1] = mode;
             _i2c.write(chip_addr, dt, 2, false);
-            WAIT_MS(7);    // wait 7mS
+            ThisThread::sleep_for(7ms);    // wait 7mS
             break;
         default:
             break;
@@ -378,7 +376,7 @@
     return dt[0];
 }
 
-/////////////// Set Mouting position  /////////////////////
+/////////////// Set Mouting position  //////////////////////////////////////////
 void BNO055::set_mounting_position(uint8_t position)
 {
     uint8_t remap_config;
@@ -429,13 +427,13 @@
     change_fusion_mode(current_mode);
 }
 
-/////////////// I2C Freq. /////////////////////////////////
+/////////////// I2C Freq. //////////////////////////////////////////////////////
 void BNO055::frequency(int hz)
 {
     _i2c.frequency(hz);
 }
 
-/////////////// Read/Write specific register //////////////
+/////////////// Read/Write specific register ///////////////////////////////////
 uint8_t BNO055::read_reg0(uint8_t addr)
 {
     select_page(0);
@@ -484,5 +482,3 @@
     change_fusion_mode(current_mode);
     return d;
 }
-
-

diff -r 07d01bf36ad0 -r b48d96169302 BNO055.h
--- a/BNO055.h	Wed Aug 23 09:44:43 2017 +0000
+++ b/BNO055.h	Wed Aug 05 05:28:13 2020 +0000
@@ -3,21 +3,25 @@
  *  BNO055 Intelligent 9-axis absolute orientation sensor
  *  by Bosch Sensortec
  *
- * Copyright (c) 2015,'17 Kenji Arai / JH1PJL
- *  http://www.page.sannet.ne.jp/kenjia/index.html
- *  http://mbed.org/users/kenjiArai/
+ * Copyright (c) 2015,'17,'20 Kenji Arai / JH1PJL
+ *  http://www7b.biglobe.ne.jp/~kenjia/
+ *  https://os.mbed.com/users/kenjiArai/
  *      Created: March     30th, 2015
- *      Revised: August    23rd, 2017
+ *      Revised: August     5th, 2020
  */
 /*
- *---------------- REFERENCE ----------------------------------------------------------------------
+ *---------------- REFERENCE ---------------------------------------------------
  * Original Information
- *  https://www.bosch-sensortec.com/en/homepage/products_3/sensor_hubs/iot_solutions/bno055_1/bno055_4
- *  Intelligent 9-axis absolute orientation sensor / Data Sheet  BST_BNO055_DS000_12 Nov. 2014 rev.1.2
+ *  https://www.bosch-sensortec.com/en/homepage/
+ *              products_3/sensor_hubs/iot_solutions/bno055_1/bno055_4
+ *  Intelligent 9-axis absolute orientation sensor
+ *              / Data Sheet  BST_BNO055_DS000_12 Nov. 2014 rev.1.2
  *  Sample software   https://github.com/BoschSensortec/BNO055_driver
  * Sensor board
- *  https://www.rutronik24.com/product/bosch+se/bno055+shuttle+board+mems/6431291.html
- *  http://microcontrollershop.com/product_info.php?products_id=7140&osCsid=10645k86db2crld4tfi0vol5g5
+ *  https://www.rutronik24.com/product/bosch+se/
+ *              bno055+shuttle+board+mems/6431291.html
+ *  http://microcontrollershop.com/product_info.php
+ *              ?products_id=7140&osCsid=10645k86db2crld4tfi0vol5g5
  */
 
 #ifndef BNO055_H
@@ -56,7 +60,7 @@
 #define I_AM_BNO055_MAG         0x32    // MAG ID
 #define I_AM_BNO055_GYR         0x0f    // GYR ID
 
-////////////// DATA TYPE DEFINITION ///////////////////////
+////////////// DATA TYPE DEFINITION ////////////////////////////////////////////
 typedef struct {
     uint8_t  chip_id;
     uint8_t  acc_id;
@@ -113,19 +117,22 @@
  * BNO055_EULER_TypeDef  euler_angles;
  *
  * int main() {
- *     pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
  *     if (imu.chip_ready() == 0){
- *         pc.printf("Bosch BNO055 is NOT avirable!!\r\n");
+ *         printf("Bosch BNO055 is NOT avirable!!\r\n");
  *     }
  *     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);
+ *     printf(
+ *      "CHIP:0x%02x, ACC:0x%02x, MAG:0x%02x, GYR:0x%02x,",
+ *      bno055_id_inf.chip_id, bno055_id_inf.acc_id,
+ *      bno055_id_inf.mag_id, bno055_id_inf.gyr_id);
+ *     printf(" , SW:0x%04x, , BL:0x%02x\r\n",
+ *            bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id)
  *     while(1) {
  *         imu.get_Euler_Angles(&euler_angles);
- *         pc.printf("Heading:%+6.1f [deg], Roll:%+6.1f [deg], Pich:%+6.1f [deg]\r\n",
- *                    euler_angles.h, euler_angles.r, euler_angles.p);
- *         wait(0.5);
+ *         printf(
+ *           "Heading:%+6.1f [deg], Roll:%+6.1f [deg], Pich:%+6.1f [deg]\r\n",
+ *           euler_angles.h, euler_angles.r, euler_angles.p);
+ *         ThisThread::sleep_for(500ms);
  *     }
  * }
  * @endcode
@@ -138,7 +145,10 @@
       * @param data SDA and SCL pins
       * @param device address
       */
-    BNO055(PinName p_sda, PinName p_scl, PinName p_reset, uint8_t addr, uint8_t mode);
+    BNO055(
+        PinName p_sda, PinName p_scl,
+        PinName p_reset, uint8_t addr, uint8_t mode
+    );
 
     /** Configure data pin
       * @param data SDA and SCL pins
@@ -281,13 +291,13 @@
 
 };
 
-//---------------------------------------------------------
-//----- Register's definition -----------------------------
-//---------------------------------------------------------
+//------------------------------------------------------------------------------
+//----- Register's definition --------------------------------------------------
+//------------------------------------------------------------------------------
 // Page id register definition
 #define BNO055_PAGE_ID          0x07
 
-//----- page0 ---------------------------------------------
+//----- page0 ------------------------------------------------------------------
 #define BNO055_CHIP_ID          0x00
 #define BNO055_ACCEL_REV_ID     0x01
 #define BNO055_MAG_REV_ID       0x02
@@ -431,7 +441,7 @@
 #define MAG_RADIUS_LSB          0x69
 #define MAG_RADIUS_MSB          0x6a
 
-//----- page1 ---------------------------------------------
+//----- page1 ------------------------------------------------------------------
 // Configuration registers
 #define ACCEL_CONFIG            0x08
 #define MAG_CONFIG              0x09
@@ -460,71 +470,76 @@
 #define GYRO_ANY_MOTION_THRES   0x1e
 #define GYRO_ANY_MOTION_SET     0x1f
 
-//---------------------------------------------------------
-//----- Calibration example -------------------------------
-//---------------------------------------------------------
+//------------------------------------------------------------------------------
+//----- Calibration example ----------------------------------------------------
+//------------------------------------------------------------------------------
 #if 0
 // Calibration
 //  Please refer BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data
-void bno055_calbration(void){
+void bno055_calbration(void)
+{
     uint8_t d;
 
-    pc.printf("------ Enter BNO055 Manual Calibration Mode ------\r\n");
+    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");
+    printf("Step1) Please wait few seconds\r\n");
     t.start();
-    while (t.read() < 10){
+    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){
+        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");
+    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");
+    printf("Step2) random moving (try to change the BNO055 axis)\r\n");
     t.start();
-    while (t.read() < 30){
+    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){
+        printf("Calb dat = 0x%x target  = 0x33(at least)\r\n", d);
+        if ((d & 0x03) == 0x03) {
             break;
         }
-        wait(1.0);   
+        wait(1.0);
     }
-    pc.printf("-> Step2) is done\r\n\r\n");
+    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);
+    printf("Step3) Change rotation each X,Y,Z axis KEEP SLOWLY!!");
+    printf(" Each 90deg stay a 5 sec and set at least 6 position.\r\n");
+    printf(" e.g. (1)ACC:X0,Y0,Z-9,(2)ACC:X9,Y0,Z0,(3)ACC:X0,Y0,Z9,");
+    printf("(4)ACC:X-9,Y0,Z0,(5)ACC:X0,Y-9,Z0,(6)ACC:X0,Y9,Z0,\r\n");
+    printf(" If you will give up, hit any key.\r\n", d);
     t.stop();
-    while (true){
+    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;}
+        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");
+    if (imu.read_calib_status() == 0xff) {
+        printf("-> All of Calibration steps are done successfully!\r\n\r\n");
     } else {
-        pc.printf("-> Calibration steps are suspended!\r\n\r\n");
+        printf("-> Calibration steps are suspended!\r\n\r\n");
     }
     t.stop();
 }