main.cpp

00001 /*
00002  * mbed Application program for the mbed Nucleo F401
00003  *  BNO055 Intelligent 9-axis absolute orientation sensor
00004  *  by Bosch Sensortec
00005  *
00006  * Copyright (c) 2015 Kenji Arai / JH1PJL
00007  *  http://www.page.sannet.ne.jp/kenjia/index.html
00008  *  http://mbed.org/users/kenjiArai/
00009  *      Created: March     30th, 2015
00010  *      Revised: April     16th, 2015
00011  *
00012  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
00013  * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
00014  * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00015  * DAMAGES OR OTHER  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00016  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00017  */
00018 
00019 //  Include ---------------------------------------------------------------------------------------
00020 #include    "mbed.h"
00021 #include    "BNO055.h"
00022 
00023 //  Definition ------------------------------------------------------------------------------------
00024 #define NUM_LOOP    100
00025 
00026 //  Object ----------------------------------------------------------------------------------------
00027 Serial pc(USBTX,USBRX);
00028 
00029 DigitalOut pwr_onoff(PB_10);
00030 I2C    i2c(D14, D15); // SDA, SCL
00031 BNO055 imu(i2c, D5);  // Reset =D5, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default
00032 
00033 Timer t;
00034 
00035 //  RAM -------------------------------------------------------------------------------------------
00036 BNO055_ID_INF_TypeDef       bno055_id_inf;
00037 BNO055_EULER_TypeDef        euler_angles;
00038 BNO055_QUATERNION_TypeDef   quaternion;
00039 BNO055_LIN_ACC_TypeDef      linear_acc;
00040 BNO055_GRAVITY_TypeDef      gravity;
00041 BNO055_TEMPERATURE_TypeDef  chip_temp;
00042 
00043 //  ROM / Constant data ---------------------------------------------------------------------------
00044 
00045 //  Function prototypes ---------------------------------------------------------------------------
00046 
00047 //-------------------------------------------------------------------------------------------------
00048 //  Control Program
00049 //-------------------------------------------------------------------------------------------------
00050 // Calibration
00051 //  Please refer BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data
00052 void bno055_calbration(void){
00053     uint8_t d;
00054  
00055     pc.printf("------ Enter BNO055 Manual Calibration Mode ------\r\n");
00056     //---------- Gyroscope Caliblation ------------------------------------------------------------
00057     // (a) Place the device in a single stable position for a period of few seconds to allow the
00058     //     gyroscope to calibrate
00059     pc.printf("Step1) Please wait few seconds\r\n");
00060     t.start();
00061     while (t.read() < 10){
00062         d = imu.read_calib_status();
00063         pc.printf("Calb dat = 0x%x target  = 0x30(at least)\r\n", d);
00064         if ((d & 0x30) == 0x30){
00065             break;
00066         }
00067         wait(1.0);
00068     }
00069     pc.printf("-> Step1) is done\r\n\r\n");
00070     //---------- Magnetometer Caliblation ---------------------------------------------------------
00071     // (a) Make some random movements (for example: writing the number ‘8’ on air) until the
00072     //     CALIB_STAT register indicates fully calibrated.
00073     // (b) It takes more calibration movements to get the magnetometer calibrated than in the
00074     //     NDOF mode.
00075     pc.printf("Step2) random moving (try to change the BNO055 axis)\r\n");
00076     t.start();
00077     while (t.read() < 30){
00078         d = imu.read_calib_status();
00079         pc.printf("Calb dat = 0x%x target  = 0x33(at least)\r\n", d);
00080         if ((d & 0x03) == 0x03){
00081             break;
00082         }
00083         wait(1.0);   
00084     }
00085     pc.printf("-> Step2) is done\r\n\r\n");
00086     //---------- Magnetometer Caliblation ---------------------------------------------------------
00087     // a) Place the device in 6 different stable positions for a period of few seconds
00088     //    to allow the accelerometer to calibrate.
00089     // b) Make sure that there is slow movement between 2 stable positions
00090     //    The 6 stable positions could be in any direction, but make sure that the device is
00091     //    lying at least once perpendicular to the x, y and z axis.
00092     pc.printf("Step3) Change rotation each X,Y,Z axis KEEP SLOWLY!!");
00093     pc.printf(" Each 90deg stay a 5 sec and set at least 6 position.\r\n");
00094     pc.printf(" e.g. (1)ACC:X0,Y0,Z-9,(2)ACC:X9,Y0,Z0,(3)ACC:X0,Y0,Z9,");
00095     pc.printf("(4)ACC:X-9,Y0,Z0,(5)ACC:X0,Y-9,Z0,(6)ACC:X0,Y9,Z0,\r\n");
00096     pc.printf(" If you will give up, hit any key.\r\n", d);
00097     t.stop();
00098     while (true){
00099         d = imu.read_calib_status();
00100         imu.get_gravity(&gravity);
00101         pc.printf("Calb dat = 0x%x target  = 0xff ACC:X %4.1f, Y %4.1f, Z %4.1f\r\n",
00102                    d, gravity.x, gravity.y, gravity.z);
00103         if (d == 0xff){     break;}
00104         if (pc.readable()){ break;}
00105         wait(1.0);
00106     }
00107     if (imu.read_calib_status() == 0xff){
00108         pc.printf("-> All of Calibration steps are done successfully!\r\n\r\n");
00109     } else {
00110         pc.printf("-> Calibration steps are suspended!\r\n\r\n");
00111     }
00112     t.stop();
00113 }
00114 
00115 int main(){
00116     pc.baud(921600);
00117     imu.set_mounting_position(MT_P6);
00118     pwr_onoff = 0;
00119     //pc.printf("\r\n\r\nIf pc terminal soft is ready, please hit any key!\r\n");
00120     //char c = pc.getc();
00121     pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
00122     // Is BNO055 avairable?
00123     if (imu.chip_ready() == 0){
00124         do {
00125             pc.printf("Bosch BNO055 is NOT avirable!!\r\n Reset\r\n");
00126             pwr_onoff = 1;  // Power off
00127             wait(0.1);
00128             pwr_onoff = 0;  // Power on
00129             wait(0.02);
00130         } while(imu.reset());
00131     }
00132     pc.printf("Bosch BNO055 is available now!!\r\n");
00133     pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n",
00134                imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN));    
00135     imu.read_id_inf(&bno055_id_inf);
00136     pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x, ",
00137         bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id);
00138     pc.printf("SW REV:0x%04x, BL REV:0x%02x\r\n",
00139         bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
00140     pc.printf("If you would like to calibrate the BNO055, please hit 'y' (No: any other key)\r\n");
00141     //c = pc.getc();
00142 //    if (c == 'y'){
00143 //        bno055_calbration();
00144 //    }
00145     pc.printf("[E]:Euler Angles[deg],[Q]:Quaternion[],[L]:Linear accel[m/s*s],");
00146     pc.printf("[G]:Gravity vector[m/s*s],[T]:Chip temperature,Acc,Gyr[degC],[S]:Status,[M]:time[mS]\r\n");
00147     t.start();
00148     while(1) {
00149         imu.get_Euler_Angles(&euler_angles);
00150         pc.printf("[E],Y,%+6.1f,R,%+6.1f,P,%+6.1f,",
00151                    euler_angles.h, euler_angles.r, euler_angles.p);
00152         imu.get_quaternion(&quaternion);
00153         pc.printf("[Q],W,%d,X,%d,Y,%d,Z,%d,",
00154                    quaternion.w, quaternion.x, quaternion.y, quaternion.z);
00155         imu.get_linear_accel(&linear_acc);
00156         pc.printf("[L],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
00157                    linear_acc.x, linear_acc.y, linear_acc.z);
00158         imu.get_gravity(&gravity);
00159         pc.printf("[G],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
00160                    gravity.x, gravity.y, gravity.z);
00161         imu.get_chip_temperature(&chip_temp);
00162         pc.printf("[T],%+d,%+d,",
00163                    chip_temp.acc_chip, chip_temp.gyr_chip);
00164         pc.printf("[S],0x%x,[M],%d\r\n",
00165                    imu.read_calib_status(), t.read_ms());
00166     }
00167 }
00168 
00169