IMU code

Dependencies:   BNO055_fusion mbed

Fork of Bosch_BNO055_Fusion_example by Kenji Arai

Committer:
EmbeddedSam
Date:
Thu Apr 27 09:41:19 2017 +0000
Revision:
6:58bb66439a03
Parent:
5:9594519c9462
Example from online, working  before Croatia;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kenjiArai 0:31451519d283 1 /*
kenjiArai 0:31451519d283 2 * mbed Application program for the mbed Nucleo F401
kenjiArai 0:31451519d283 3 * BNO055 Intelligent 9-axis absolute orientation sensor
kenjiArai 0:31451519d283 4 * by Bosch Sensortec
kenjiArai 0:31451519d283 5 *
kenjiArai 0:31451519d283 6 * Copyright (c) 2015 Kenji Arai / JH1PJL
kenjiArai 0:31451519d283 7 * http://www.page.sannet.ne.jp/kenjia/index.html
kenjiArai 0:31451519d283 8 * http://mbed.org/users/kenjiArai/
kenjiArai 0:31451519d283 9 * Created: March 30th, 2015
kenjiArai 5:9594519c9462 10 * Revised: April 16th, 2015
kenjiArai 0:31451519d283 11 *
kenjiArai 0:31451519d283 12 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
kenjiArai 0:31451519d283 13 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
kenjiArai 0:31451519d283 14 * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
kenjiArai 0:31451519d283 15 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
kenjiArai 0:31451519d283 16 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
kenjiArai 0:31451519d283 17 */
kenjiArai 0:31451519d283 18
kenjiArai 0:31451519d283 19 // Include ---------------------------------------------------------------------------------------
kenjiArai 0:31451519d283 20 #include "mbed.h"
kenjiArai 0:31451519d283 21 #include "BNO055.h"
kenjiArai 0:31451519d283 22
kenjiArai 0:31451519d283 23 // Definition ------------------------------------------------------------------------------------
kenjiArai 3:f5b5c4d795ce 24 #define NUM_LOOP 100
kenjiArai 3:f5b5c4d795ce 25
kenjiArai 0:31451519d283 26 // Object ----------------------------------------------------------------------------------------
kenjiArai 0:31451519d283 27 Serial pc(USBTX,USBRX);
EmbeddedSam 6:58bb66439a03 28
kenjiArai 2:cf77282aea7b 29 DigitalOut pwr_onoff(PB_10);
EmbeddedSam 6:58bb66439a03 30 I2C i2c(D14, D15); // SDA, SCL
EmbeddedSam 6:58bb66439a03 31 BNO055 imu(i2c, D5); // Reset =D5, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default
EmbeddedSam 6:58bb66439a03 32
kenjiArai 3:f5b5c4d795ce 33 Timer t;
kenjiArai 0:31451519d283 34
kenjiArai 0:31451519d283 35 // RAM -------------------------------------------------------------------------------------------
kenjiArai 0:31451519d283 36 BNO055_ID_INF_TypeDef bno055_id_inf;
kenjiArai 0:31451519d283 37 BNO055_EULER_TypeDef euler_angles;
kenjiArai 0:31451519d283 38 BNO055_QUATERNION_TypeDef quaternion;
kenjiArai 0:31451519d283 39 BNO055_LIN_ACC_TypeDef linear_acc;
kenjiArai 0:31451519d283 40 BNO055_GRAVITY_TypeDef gravity;
kenjiArai 0:31451519d283 41 BNO055_TEMPERATURE_TypeDef chip_temp;
kenjiArai 0:31451519d283 42
kenjiArai 0:31451519d283 43 // ROM / Constant data ---------------------------------------------------------------------------
kenjiArai 0:31451519d283 44
kenjiArai 0:31451519d283 45 // Function prototypes ---------------------------------------------------------------------------
kenjiArai 0:31451519d283 46
kenjiArai 0:31451519d283 47 //-------------------------------------------------------------------------------------------------
kenjiArai 0:31451519d283 48 // Control Program
kenjiArai 3:f5b5c4d795ce 49 //-------------------------------------------------------------------------------------------------
kenjiArai 4:6d1118089a36 50 // Calibration
kenjiArai 4:6d1118089a36 51 // Please refer BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data
kenjiArai 4:6d1118089a36 52 void bno055_calbration(void){
kenjiArai 4:6d1118089a36 53 uint8_t d;
EmbeddedSam 6:58bb66439a03 54
kenjiArai 4:6d1118089a36 55 pc.printf("------ Enter BNO055 Manual Calibration Mode ------\r\n");
kenjiArai 4:6d1118089a36 56 //---------- Gyroscope Caliblation ------------------------------------------------------------
kenjiArai 4:6d1118089a36 57 // (a) Place the device in a single stable position for a period of few seconds to allow the
kenjiArai 4:6d1118089a36 58 // gyroscope to calibrate
kenjiArai 4:6d1118089a36 59 pc.printf("Step1) Please wait few seconds\r\n");
kenjiArai 4:6d1118089a36 60 t.start();
kenjiArai 4:6d1118089a36 61 while (t.read() < 10){
kenjiArai 4:6d1118089a36 62 d = imu.read_calib_status();
kenjiArai 4:6d1118089a36 63 pc.printf("Calb dat = 0x%x target = 0x30(at least)\r\n", d);
kenjiArai 4:6d1118089a36 64 if ((d & 0x30) == 0x30){
kenjiArai 4:6d1118089a36 65 break;
kenjiArai 4:6d1118089a36 66 }
kenjiArai 4:6d1118089a36 67 wait(1.0);
kenjiArai 4:6d1118089a36 68 }
kenjiArai 4:6d1118089a36 69 pc.printf("-> Step1) is done\r\n\r\n");
kenjiArai 4:6d1118089a36 70 //---------- Magnetometer Caliblation ---------------------------------------------------------
kenjiArai 4:6d1118089a36 71 // (a) Make some random movements (for example: writing the number ‘8’ on air) until the
kenjiArai 4:6d1118089a36 72 // CALIB_STAT register indicates fully calibrated.
kenjiArai 4:6d1118089a36 73 // (b) It takes more calibration movements to get the magnetometer calibrated than in the
kenjiArai 4:6d1118089a36 74 // NDOF mode.
kenjiArai 4:6d1118089a36 75 pc.printf("Step2) random moving (try to change the BNO055 axis)\r\n");
kenjiArai 4:6d1118089a36 76 t.start();
kenjiArai 4:6d1118089a36 77 while (t.read() < 30){
kenjiArai 4:6d1118089a36 78 d = imu.read_calib_status();
kenjiArai 4:6d1118089a36 79 pc.printf("Calb dat = 0x%x target = 0x33(at least)\r\n", d);
kenjiArai 4:6d1118089a36 80 if ((d & 0x03) == 0x03){
kenjiArai 4:6d1118089a36 81 break;
kenjiArai 4:6d1118089a36 82 }
kenjiArai 4:6d1118089a36 83 wait(1.0);
kenjiArai 4:6d1118089a36 84 }
kenjiArai 4:6d1118089a36 85 pc.printf("-> Step2) is done\r\n\r\n");
kenjiArai 4:6d1118089a36 86 //---------- Magnetometer Caliblation ---------------------------------------------------------
kenjiArai 4:6d1118089a36 87 // a) Place the device in 6 different stable positions for a period of few seconds
kenjiArai 4:6d1118089a36 88 // to allow the accelerometer to calibrate.
kenjiArai 4:6d1118089a36 89 // b) Make sure that there is slow movement between 2 stable positions
kenjiArai 4:6d1118089a36 90 // The 6 stable positions could be in any direction, but make sure that the device is
kenjiArai 4:6d1118089a36 91 // lying at least once perpendicular to the x, y and z axis.
kenjiArai 4:6d1118089a36 92 pc.printf("Step3) Change rotation each X,Y,Z axis KEEP SLOWLY!!");
kenjiArai 4:6d1118089a36 93 pc.printf(" Each 90deg stay a 5 sec and set at least 6 position.\r\n");
kenjiArai 4:6d1118089a36 94 pc.printf(" e.g. (1)ACC:X0,Y0,Z-9,(2)ACC:X9,Y0,Z0,(3)ACC:X0,Y0,Z9,");
kenjiArai 4:6d1118089a36 95 pc.printf("(4)ACC:X-9,Y0,Z0,(5)ACC:X0,Y-9,Z0,(6)ACC:X0,Y9,Z0,\r\n");
kenjiArai 4:6d1118089a36 96 pc.printf(" If you will give up, hit any key.\r\n", d);
kenjiArai 4:6d1118089a36 97 t.stop();
kenjiArai 4:6d1118089a36 98 while (true){
kenjiArai 4:6d1118089a36 99 d = imu.read_calib_status();
kenjiArai 4:6d1118089a36 100 imu.get_gravity(&gravity);
kenjiArai 5:9594519c9462 101 pc.printf("Calb dat = 0x%x target = 0xff ACC:X %4.1f, Y %4.1f, Z %4.1f\r\n",
kenjiArai 4:6d1118089a36 102 d, gravity.x, gravity.y, gravity.z);
kenjiArai 4:6d1118089a36 103 if (d == 0xff){ break;}
kenjiArai 4:6d1118089a36 104 if (pc.readable()){ break;}
kenjiArai 4:6d1118089a36 105 wait(1.0);
kenjiArai 4:6d1118089a36 106 }
kenjiArai 4:6d1118089a36 107 if (imu.read_calib_status() == 0xff){
kenjiArai 4:6d1118089a36 108 pc.printf("-> All of Calibration steps are done successfully!\r\n\r\n");
kenjiArai 4:6d1118089a36 109 } else {
kenjiArai 4:6d1118089a36 110 pc.printf("-> Calibration steps are suspended!\r\n\r\n");
kenjiArai 4:6d1118089a36 111 }
kenjiArai 4:6d1118089a36 112 t.stop();
kenjiArai 4:6d1118089a36 113 }
kenjiArai 4:6d1118089a36 114
kenjiArai 4:6d1118089a36 115 int main(){
EmbeddedSam 6:58bb66439a03 116 pc.baud(921600);
kenjiArai 4:6d1118089a36 117 imu.set_mounting_position(MT_P6);
kenjiArai 4:6d1118089a36 118 pwr_onoff = 0;
EmbeddedSam 6:58bb66439a03 119 //pc.printf("\r\n\r\nIf pc terminal soft is ready, please hit any key!\r\n");
EmbeddedSam 6:58bb66439a03 120 //char c = pc.getc();
kenjiArai 4:6d1118089a36 121 pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
kenjiArai 4:6d1118089a36 122 // Is BNO055 avairable?
kenjiArai 4:6d1118089a36 123 if (imu.chip_ready() == 0){
kenjiArai 4:6d1118089a36 124 do {
kenjiArai 4:6d1118089a36 125 pc.printf("Bosch BNO055 is NOT avirable!!\r\n Reset\r\n");
kenjiArai 4:6d1118089a36 126 pwr_onoff = 1; // Power off
kenjiArai 4:6d1118089a36 127 wait(0.1);
kenjiArai 4:6d1118089a36 128 pwr_onoff = 0; // Power on
kenjiArai 4:6d1118089a36 129 wait(0.02);
kenjiArai 4:6d1118089a36 130 } while(imu.reset());
kenjiArai 4:6d1118089a36 131 }
kenjiArai 4:6d1118089a36 132 pc.printf("Bosch BNO055 is available now!!\r\n");
kenjiArai 4:6d1118089a36 133 pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n",
kenjiArai 4:6d1118089a36 134 imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN));
kenjiArai 4:6d1118089a36 135 imu.read_id_inf(&bno055_id_inf);
kenjiArai 4:6d1118089a36 136 pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x, ",
kenjiArai 4:6d1118089a36 137 bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id);
kenjiArai 4:6d1118089a36 138 pc.printf("SW REV:0x%04x, BL REV:0x%02x\r\n",
kenjiArai 4:6d1118089a36 139 bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
kenjiArai 4:6d1118089a36 140 pc.printf("If you would like to calibrate the BNO055, please hit 'y' (No: any other key)\r\n");
EmbeddedSam 6:58bb66439a03 141 //c = pc.getc();
EmbeddedSam 6:58bb66439a03 142 // if (c == 'y'){
EmbeddedSam 6:58bb66439a03 143 // bno055_calbration();
EmbeddedSam 6:58bb66439a03 144 // }
kenjiArai 4:6d1118089a36 145 pc.printf("[E]:Euler Angles[deg],[Q]:Quaternion[],[L]:Linear accel[m/s*s],");
kenjiArai 4:6d1118089a36 146 pc.printf("[G]:Gravity vector[m/s*s],[T]:Chip temperature,Acc,Gyr[degC],[S]:Status,[M]:time[mS]\r\n");
kenjiArai 4:6d1118089a36 147 t.start();
kenjiArai 4:6d1118089a36 148 while(1) {
kenjiArai 4:6d1118089a36 149 imu.get_Euler_Angles(&euler_angles);
kenjiArai 4:6d1118089a36 150 pc.printf("[E],Y,%+6.1f,R,%+6.1f,P,%+6.1f,",
kenjiArai 4:6d1118089a36 151 euler_angles.h, euler_angles.r, euler_angles.p);
kenjiArai 4:6d1118089a36 152 imu.get_quaternion(&quaternion);
kenjiArai 4:6d1118089a36 153 pc.printf("[Q],W,%d,X,%d,Y,%d,Z,%d,",
kenjiArai 4:6d1118089a36 154 quaternion.w, quaternion.x, quaternion.y, quaternion.z);
kenjiArai 4:6d1118089a36 155 imu.get_linear_accel(&linear_acc);
kenjiArai 4:6d1118089a36 156 pc.printf("[L],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
kenjiArai 4:6d1118089a36 157 linear_acc.x, linear_acc.y, linear_acc.z);
kenjiArai 4:6d1118089a36 158 imu.get_gravity(&gravity);
kenjiArai 4:6d1118089a36 159 pc.printf("[G],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
kenjiArai 4:6d1118089a36 160 gravity.x, gravity.y, gravity.z);
kenjiArai 4:6d1118089a36 161 imu.get_chip_temperature(&chip_temp);
kenjiArai 4:6d1118089a36 162 pc.printf("[T],%+d,%+d,",
kenjiArai 4:6d1118089a36 163 chip_temp.acc_chip, chip_temp.gyr_chip);
kenjiArai 4:6d1118089a36 164 pc.printf("[S],0x%x,[M],%d\r\n",
kenjiArai 4:6d1118089a36 165 imu.read_calib_status(), t.read_ms());
kenjiArai 4:6d1118089a36 166 }
kenjiArai 4:6d1118089a36 167 }
kenjiArai 4:6d1118089a36 168
kenjiArai 4:6d1118089a36 169