Based on F401 example.Changed reset sequence and added RESET control and Power On/Off control. Check several mbed, LPC1768, LPC1114, NucleoF401RE, F411RE, L152RE and GR-PEACH
Dependencies: BNO055_fusion TextLCD
Please see follows.
/users/kenjiArai/notebook/bno055---orientation-sensor/
main.cpp@7:f244ea2ab994, 2020-08-05 (annotated)
- Committer:
- kenjiArai
- Date:
- Wed Aug 05 05:29:12 2020 +0000
- Revision:
- 7:f244ea2ab994
- Parent:
- 6:5f380fbcf849
run on mbed-os6.2.0
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kenjiArai | 0:31451519d283 | 1 | /* |
kenjiArai | 7:f244ea2ab994 | 2 | * mbed Application program for the mbed Nucleo series |
kenjiArai | 0:31451519d283 | 3 | * BNO055 Intelligent 9-axis absolute orientation sensor |
kenjiArai | 0:31451519d283 | 4 | * by Bosch Sensortec |
kenjiArai | 0:31451519d283 | 5 | * |
kenjiArai | 7:f244ea2ab994 | 6 | * Copyright (c) 2015,'17,'20 Kenji Arai / JH1PJL |
kenjiArai | 7:f244ea2ab994 | 7 | * http://www7b.biglobe.ne.jp/~kenjia/ |
kenjiArai | 7:f244ea2ab994 | 8 | * https://os.mbed.com/users/kenjiArai/ |
kenjiArai | 0:31451519d283 | 9 | * Created: March 30th, 2015 |
kenjiArai | 7:f244ea2ab994 | 10 | * Revised: August 5th, 2020 |
kenjiArai | 0:31451519d283 | 11 | */ |
kenjiArai | 0:31451519d283 | 12 | |
kenjiArai | 7:f244ea2ab994 | 13 | // Include -------------------------------------------------------------------- |
kenjiArai | 0:31451519d283 | 14 | #include "mbed.h" |
kenjiArai | 0:31451519d283 | 15 | #include "BNO055.h" |
kenjiArai | 6:5f380fbcf849 | 16 | #include "TextLCD.h" |
kenjiArai | 0:31451519d283 | 17 | |
kenjiArai | 7:f244ea2ab994 | 18 | // Definition ----------------------------------------------------------------- |
kenjiArai | 3:f5b5c4d795ce | 19 | #define NUM_LOOP 100 |
kenjiArai | 3:f5b5c4d795ce | 20 | |
kenjiArai | 7:f244ea2ab994 | 21 | // Object --------------------------------------------------------------------- |
kenjiArai | 7:f244ea2ab994 | 22 | static BufferedSerial pc(USBTX, USBRX, 9600); |
kenjiArai | 2:cf77282aea7b | 23 | #if defined(TARGET_LPC1114) |
kenjiArai | 2:cf77282aea7b | 24 | DigitalOut pwr_onoff(dp17); |
kenjiArai | 2:cf77282aea7b | 25 | I2C i2c(dp5, dp27); // SDA, SCL |
kenjiArai | 7:f244ea2ab994 | 26 | // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default |
kenjiArai | 7:f244ea2ab994 | 27 | BNO055 imu(i2c, dp18); |
kenjiArai | 2:cf77282aea7b | 28 | #elif defined(TARGET_LPC1768) |
kenjiArai | 2:cf77282aea7b | 29 | DigitalOut pwr_onoff(p30); |
kenjiArai | 2:cf77282aea7b | 30 | I2C i2c(p28, p27); // SDA, SCL |
kenjiArai | 7:f244ea2ab994 | 31 | // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default |
kenjiArai | 7:f244ea2ab994 | 32 | BNO055 imu(i2c, p29); |
kenjiArai | 7:f244ea2ab994 | 33 | #elif defined(TARGET_NUCLEO_L152RE)\ |
kenjiArai | 7:f244ea2ab994 | 34 | || defined(TARGET_NUCLEO_F401RE)\ |
kenjiArai | 7:f244ea2ab994 | 35 | || defined(TARGET_NUCLEO_F411RE)\ |
kenjiArai | 7:f244ea2ab994 | 36 | || defined(TARGET_NUCLEO_F446RE) |
kenjiArai | 6:5f380fbcf849 | 37 | #if 0 |
kenjiArai | 2:cf77282aea7b | 38 | DigitalOut pwr_onoff(PB_10); |
kenjiArai | 6:5f380fbcf849 | 39 | #else |
kenjiArai | 6:5f380fbcf849 | 40 | DigitalOut pwr_onoff(PA_9); |
kenjiArai | 6:5f380fbcf849 | 41 | #endif |
kenjiArai | 0:31451519d283 | 42 | I2C i2c(PB_9, PB_8); // SDA, SCL |
kenjiArai | 6:5f380fbcf849 | 43 | #if 0 |
kenjiArai | 7:f244ea2ab994 | 44 | // Reset = ??, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default |
kenjiArai | 7:f244ea2ab994 | 45 | BNO055 imu(i2c, PA_8); |
kenjiArai | 6:5f380fbcf849 | 46 | #else |
kenjiArai | 6:5f380fbcf849 | 47 | BNO055 imu(PB_9, PB_8, PA_8); |
kenjiArai | 6:5f380fbcf849 | 48 | #endif |
kenjiArai | 6:5f380fbcf849 | 49 | TextLCD_I2C_N lcd(&i2c, 0x7c, TextLCD::LCD8x2); // LCD(Akizuki AQM0802A) |
kenjiArai | 2:cf77282aea7b | 50 | #elif defined(TARGET_RZ_A1H) |
kenjiArai | 2:cf77282aea7b | 51 | DigitalOut pwr_onoff(P8_11); |
kenjiArai | 2:cf77282aea7b | 52 | I2C i2c(P1_3, P1_2); // SDA, SCL |
kenjiArai | 7:f244ea2ab994 | 53 | // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default |
kenjiArai | 7:f244ea2ab994 | 54 | BNO055 imu(i2c, P8_13); |
kenjiArai | 2:cf77282aea7b | 55 | #else |
kenjiArai | 2:cf77282aea7b | 56 | #error "Not cheched yet" |
kenjiArai | 2:cf77282aea7b | 57 | #endif |
kenjiArai | 3:f5b5c4d795ce | 58 | Timer t; |
kenjiArai | 0:31451519d283 | 59 | |
kenjiArai | 7:f244ea2ab994 | 60 | // RAM ------------------------------------------------------------------------ |
kenjiArai | 0:31451519d283 | 61 | BNO055_ID_INF_TypeDef bno055_id_inf; |
kenjiArai | 0:31451519d283 | 62 | BNO055_EULER_TypeDef euler_angles; |
kenjiArai | 0:31451519d283 | 63 | BNO055_QUATERNION_TypeDef quaternion; |
kenjiArai | 0:31451519d283 | 64 | BNO055_LIN_ACC_TypeDef linear_acc; |
kenjiArai | 0:31451519d283 | 65 | BNO055_GRAVITY_TypeDef gravity; |
kenjiArai | 0:31451519d283 | 66 | BNO055_TEMPERATURE_TypeDef chip_temp; |
kenjiArai | 0:31451519d283 | 67 | |
kenjiArai | 7:f244ea2ab994 | 68 | // ROM / Constant data -------------------------------------------------------- |
kenjiArai | 0:31451519d283 | 69 | |
kenjiArai | 7:f244ea2ab994 | 70 | // Function prototypes -------------------------------------------------------- |
kenjiArai | 0:31451519d283 | 71 | |
kenjiArai | 7:f244ea2ab994 | 72 | //------------------------------------------------------------------------------ |
kenjiArai | 0:31451519d283 | 73 | // Control Program |
kenjiArai | 7:f244ea2ab994 | 74 | //------------------------------------------------------------------------------ |
kenjiArai | 4:6d1118089a36 | 75 | // Calibration |
kenjiArai | 7:f244ea2ab994 | 76 | // Please refer |
kenjiArai | 7:f244ea2ab994 | 77 | // BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data |
kenjiArai | 7:f244ea2ab994 | 78 | void bno055_calbration(void) |
kenjiArai | 7:f244ea2ab994 | 79 | { |
kenjiArai | 4:6d1118089a36 | 80 | uint8_t d; |
kenjiArai | 3:f5b5c4d795ce | 81 | |
kenjiArai | 7:f244ea2ab994 | 82 | printf("------ Enter BNO055 Manual Calibration Mode ------\r\n"); |
kenjiArai | 7:f244ea2ab994 | 83 | //---------- Gyroscope Caliblation ----------------------------------------- |
kenjiArai | 7:f244ea2ab994 | 84 | // (a) Place the device in a single stable position for a period of |
kenjiArai | 7:f244ea2ab994 | 85 | // few seconds to allow the gyroscope to calibrate |
kenjiArai | 7:f244ea2ab994 | 86 | printf("Step1) Please wait few seconds\r\n"); |
kenjiArai | 4:6d1118089a36 | 87 | t.start(); |
kenjiArai | 7:f244ea2ab994 | 88 | while (t.elapsed_time().count() < 10) { |
kenjiArai | 4:6d1118089a36 | 89 | d = imu.read_calib_status(); |
kenjiArai | 7:f244ea2ab994 | 90 | printf("Calb dat = 0x%x target = 0x30(at least)\r\n", d); |
kenjiArai | 7:f244ea2ab994 | 91 | if ((d & 0x30) == 0x30) { |
kenjiArai | 4:6d1118089a36 | 92 | break; |
kenjiArai | 4:6d1118089a36 | 93 | } |
kenjiArai | 7:f244ea2ab994 | 94 | ThisThread::sleep_for(1s); |
kenjiArai | 4:6d1118089a36 | 95 | } |
kenjiArai | 7:f244ea2ab994 | 96 | printf("-> Step1) is done\r\n\r\n"); |
kenjiArai | 7:f244ea2ab994 | 97 | //---------- Magnetometer Caliblation -------------------------------------- |
kenjiArai | 7:f244ea2ab994 | 98 | // (a) Make some random movements (for example: writing the number ‘8’ |
kenjiArai | 7:f244ea2ab994 | 99 | // on air) until the CALIB_STAT register indicates fully calibrated. |
kenjiArai | 7:f244ea2ab994 | 100 | // (b) It takes more calibration movements to get the magnetometer |
kenjiArai | 7:f244ea2ab994 | 101 | // calibrated than in the NDOF mode. |
kenjiArai | 7:f244ea2ab994 | 102 | printf("Step2) random moving (try to change the BNO055 axis)\r\n"); |
kenjiArai | 4:6d1118089a36 | 103 | t.start(); |
kenjiArai | 7:f244ea2ab994 | 104 | while (t.elapsed_time().count() < 30) { |
kenjiArai | 4:6d1118089a36 | 105 | d = imu.read_calib_status(); |
kenjiArai | 7:f244ea2ab994 | 106 | printf("Calb dat = 0x%x target = 0x33(at least)\r\n", d); |
kenjiArai | 7:f244ea2ab994 | 107 | if ((d & 0x03) == 0x03) { |
kenjiArai | 4:6d1118089a36 | 108 | break; |
kenjiArai | 4:6d1118089a36 | 109 | } |
kenjiArai | 7:f244ea2ab994 | 110 | ThisThread::sleep_for(1s); |
kenjiArai | 4:6d1118089a36 | 111 | } |
kenjiArai | 7:f244ea2ab994 | 112 | printf("-> Step2) is done\r\n\r\n"); |
kenjiArai | 7:f244ea2ab994 | 113 | //---------- Magnetometer Caliblation -------------------------------------- |
kenjiArai | 7:f244ea2ab994 | 114 | // a) Place the device in 6 different stable positions for a period of |
kenjiArai | 7:f244ea2ab994 | 115 | // few seconds to allow the accelerometer to calibrate. |
kenjiArai | 4:6d1118089a36 | 116 | // b) Make sure that there is slow movement between 2 stable positions |
kenjiArai | 7:f244ea2ab994 | 117 | // The 6 stable positions could be in any direction, but make sure that |
kenjiArai | 7:f244ea2ab994 | 118 | // the device is lying at least once perpendicular to the x, y and z axis |
kenjiArai | 7:f244ea2ab994 | 119 | printf("Step3) Change rotation each X,Y,Z axis KEEP SLOWLY!!"); |
kenjiArai | 7:f244ea2ab994 | 120 | printf(" Each 90deg stay a 5 sec and set at least 6 position.\r\n"); |
kenjiArai | 7:f244ea2ab994 | 121 | printf(" e.g. (1)ACC:X0,Y0,Z-9,(2)ACC:X9,Y0,Z0,(3)ACC:X0,Y0,Z9,"); |
kenjiArai | 7:f244ea2ab994 | 122 | printf("(4)ACC:X-9,Y0,Z0,(5)ACC:X0,Y-9,Z0,(6)ACC:X0,Y9,Z0,\r\n"); |
kenjiArai | 7:f244ea2ab994 | 123 | printf(" If you will give up, hit any key.\r\n"); |
kenjiArai | 4:6d1118089a36 | 124 | t.stop(); |
kenjiArai | 6:5f380fbcf849 | 125 | |
kenjiArai | 6:5f380fbcf849 | 126 | // lcd |
kenjiArai | 6:5f380fbcf849 | 127 | lcd.locate(0, 0); // 1st line top |
kenjiArai | 6:5f380fbcf849 | 128 | // 12345678 |
kenjiArai | 6:5f380fbcf849 | 129 | lcd.printf(" BNO055 "); |
kenjiArai | 6:5f380fbcf849 | 130 | lcd.locate(0, 1); // 2nd line top |
kenjiArai | 6:5f380fbcf849 | 131 | // 12345678 |
kenjiArai | 6:5f380fbcf849 | 132 | lcd.puts(" JH1PJL "); |
kenjiArai | 6:5f380fbcf849 | 133 | lcd.setContrast(0x14); |
kenjiArai | 6:5f380fbcf849 | 134 | |
kenjiArai | 7:f244ea2ab994 | 135 | while (true) { |
kenjiArai | 4:6d1118089a36 | 136 | d = imu.read_calib_status(); |
kenjiArai | 4:6d1118089a36 | 137 | imu.get_gravity(&gravity); |
kenjiArai | 7:f244ea2ab994 | 138 | printf( |
kenjiArai | 7:f244ea2ab994 | 139 | "Calb dat = 0x%x target = 0xff ACC:X %4.1f, Y %4.1f, Z %4.1f\r\n", |
kenjiArai | 7:f244ea2ab994 | 140 | d, gravity.x, gravity.y, gravity.z |
kenjiArai | 7:f244ea2ab994 | 141 | ); |
kenjiArai | 7:f244ea2ab994 | 142 | if (d == 0xff) { |
kenjiArai | 7:f244ea2ab994 | 143 | break; |
kenjiArai | 7:f244ea2ab994 | 144 | } |
kenjiArai | 7:f244ea2ab994 | 145 | if (pc.readable()) { |
kenjiArai | 7:f244ea2ab994 | 146 | break; |
kenjiArai | 7:f244ea2ab994 | 147 | } |
kenjiArai | 7:f244ea2ab994 | 148 | ThisThread::sleep_for(1s); |
kenjiArai | 4:6d1118089a36 | 149 | } |
kenjiArai | 7:f244ea2ab994 | 150 | if (imu.read_calib_status() == 0xff) { |
kenjiArai | 7:f244ea2ab994 | 151 | printf("-> All of Calibration steps are done successfully!\r\n\r\n"); |
kenjiArai | 4:6d1118089a36 | 152 | } else { |
kenjiArai | 7:f244ea2ab994 | 153 | printf("-> Calibration steps are suspended!\r\n\r\n"); |
kenjiArai | 4:6d1118089a36 | 154 | } |
kenjiArai | 4:6d1118089a36 | 155 | t.stop(); |
kenjiArai | 4:6d1118089a36 | 156 | } |
kenjiArai | 4:6d1118089a36 | 157 | |
kenjiArai | 7:f244ea2ab994 | 158 | int main() |
kenjiArai | 7:f244ea2ab994 | 159 | { |
kenjiArai | 7:f244ea2ab994 | 160 | uint8_t ser_buf[4]; |
kenjiArai | 7:f244ea2ab994 | 161 | |
kenjiArai | 4:6d1118089a36 | 162 | imu.set_mounting_position(MT_P6); |
kenjiArai | 6:5f380fbcf849 | 163 | pwr_onoff = 1; |
kenjiArai | 7:f244ea2ab994 | 164 | printf("\r\n\r\nIf pc terminal soft is ready, please hit any key!\r\n"); |
kenjiArai | 7:f244ea2ab994 | 165 | while (pc.readable() == 0) {;} |
kenjiArai | 7:f244ea2ab994 | 166 | printf( |
kenjiArai | 7:f244ea2ab994 | 167 | "Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n" |
kenjiArai | 7:f244ea2ab994 | 168 | ); |
kenjiArai | 4:6d1118089a36 | 169 | // Is BNO055 avairable? |
kenjiArai | 7:f244ea2ab994 | 170 | if (imu.chip_ready() == 0) { |
kenjiArai | 4:6d1118089a36 | 171 | do { |
kenjiArai | 7:f244ea2ab994 | 172 | printf("Bosch BNO055 is NOT avirable!!\r\n Reset\r\n"); |
kenjiArai | 6:5f380fbcf849 | 173 | pwr_onoff = 0; // Power off |
kenjiArai | 7:f244ea2ab994 | 174 | ThisThread::sleep_for(100ms); |
kenjiArai | 6:5f380fbcf849 | 175 | pwr_onoff = 1; // Power on |
kenjiArai | 7:f244ea2ab994 | 176 | ThisThread::sleep_for(20ms); |
kenjiArai | 4:6d1118089a36 | 177 | } while(imu.reset()); |
kenjiArai | 4:6d1118089a36 | 178 | } |
kenjiArai | 7:f244ea2ab994 | 179 | printf("Bosch BNO055 is available now!!\r\n"); |
kenjiArai | 7:f244ea2ab994 | 180 | printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n", |
kenjiArai | 7:f244ea2ab994 | 181 | imu.read_reg0(BNO055_AXIS_MAP_CONFIG), |
kenjiArai | 7:f244ea2ab994 | 182 | imu.read_reg0(BNO055_AXIS_MAP_SIGN) |
kenjiArai | 7:f244ea2ab994 | 183 | ); |
kenjiArai | 4:6d1118089a36 | 184 | imu.read_id_inf(&bno055_id_inf); |
kenjiArai | 7:f244ea2ab994 | 185 | printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x, ", |
kenjiArai | 7:f244ea2ab994 | 186 | bno055_id_inf.chip_id, bno055_id_inf.acc_id, |
kenjiArai | 7:f244ea2ab994 | 187 | bno055_id_inf.mag_id, bno055_id_inf.gyr_id |
kenjiArai | 7:f244ea2ab994 | 188 | ); |
kenjiArai | 7:f244ea2ab994 | 189 | printf("SW REV:0x%04x, BL REV:0x%02x\r\n", |
kenjiArai | 7:f244ea2ab994 | 190 | bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); |
kenjiArai | 7:f244ea2ab994 | 191 | while (pc.readable() == 1) { |
kenjiArai | 7:f244ea2ab994 | 192 | pc.read(ser_buf, 1); |
kenjiArai | 7:f244ea2ab994 | 193 | } |
kenjiArai | 7:f244ea2ab994 | 194 | printf("If you would like to calibrate the BNO055,"); |
kenjiArai | 7:f244ea2ab994 | 195 | printf(" please hit 'y' (No: any other key)\r\n"); |
kenjiArai | 7:f244ea2ab994 | 196 | while (pc.readable() == 0) {;} |
kenjiArai | 7:f244ea2ab994 | 197 | pc.read(ser_buf, 1); |
kenjiArai | 7:f244ea2ab994 | 198 | uint8_t c = ser_buf[0]; |
kenjiArai | 7:f244ea2ab994 | 199 | if (c == 'y') { |
kenjiArai | 4:6d1118089a36 | 200 | bno055_calbration(); |
kenjiArai | 4:6d1118089a36 | 201 | } |
kenjiArai | 7:f244ea2ab994 | 202 | printf("[E]:Euler Angles[deg],[Q]:Quaternion[],[L]:Linear accel[m/s*s],"); |
kenjiArai | 7:f244ea2ab994 | 203 | printf("[G]:Gravity vector[m/s*s],[T]:Chip temperature,Acc,Gyr[degC]"); |
kenjiArai | 7:f244ea2ab994 | 204 | printf(",[S]:Status,[M]:time[mS]\r\n"); |
kenjiArai | 4:6d1118089a36 | 205 | t.start(); |
kenjiArai | 7:f244ea2ab994 | 206 | while(true) { |
kenjiArai | 4:6d1118089a36 | 207 | imu.get_Euler_Angles(&euler_angles); |
kenjiArai | 7:f244ea2ab994 | 208 | printf("[E],Y,%+6.1f,R,%+6.1f,P,%+6.1f,", |
kenjiArai | 7:f244ea2ab994 | 209 | euler_angles.h, euler_angles.r, euler_angles.p); |
kenjiArai | 4:6d1118089a36 | 210 | imu.get_quaternion(&quaternion); |
kenjiArai | 7:f244ea2ab994 | 211 | printf("[Q],W,%d,X,%d,Y,%d,Z,%d,", |
kenjiArai | 7:f244ea2ab994 | 212 | quaternion.w, quaternion.x, quaternion.y, quaternion.z); |
kenjiArai | 4:6d1118089a36 | 213 | imu.get_linear_accel(&linear_acc); |
kenjiArai | 7:f244ea2ab994 | 214 | printf("[L],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", |
kenjiArai | 7:f244ea2ab994 | 215 | linear_acc.x, linear_acc.y, linear_acc.z); |
kenjiArai | 4:6d1118089a36 | 216 | imu.get_gravity(&gravity); |
kenjiArai | 7:f244ea2ab994 | 217 | printf("[G],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", |
kenjiArai | 7:f244ea2ab994 | 218 | gravity.x, gravity.y, gravity.z); |
kenjiArai | 4:6d1118089a36 | 219 | imu.get_chip_temperature(&chip_temp); |
kenjiArai | 7:f244ea2ab994 | 220 | printf("[T],%+d,%+d,", |
kenjiArai | 7:f244ea2ab994 | 221 | chip_temp.acc_chip, chip_temp.gyr_chip); |
kenjiArai | 7:f244ea2ab994 | 222 | printf("[S],0x%x,[M],%d\r\n", |
kenjiArai | 7:f244ea2ab994 | 223 | imu.read_calib_status(), (uint32_t)t.elapsed_time().count()); |
kenjiArai | 4:6d1118089a36 | 224 | } |
kenjiArai | 4:6d1118089a36 | 225 | } |
kenjiArai | 4:6d1118089a36 | 226 | |
kenjiArai | 4:6d1118089a36 | 227 | |
kenjiArai | 4:6d1118089a36 | 228 | // Diffrent output format as for your reference |
kenjiArai | 4:6d1118089a36 | 229 | #if 0 |
kenjiArai | 7:f244ea2ab994 | 230 | int main() |
kenjiArai | 7:f244ea2ab994 | 231 | { |
kenjiArai | 0:31451519d283 | 232 | uint8_t i; |
kenjiArai | 0:31451519d283 | 233 | |
kenjiArai | 6:5f380fbcf849 | 234 | pwr_onoff = 1; |
kenjiArai | 7:f244ea2ab994 | 235 | printf( |
kenjiArai | 7:f244ea2ab994 | 236 | "Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n" |
kenjiArai | 7:f244ea2ab994 | 237 | ); |
kenjiArai | 0:31451519d283 | 238 | // Is BNO055 avairable? |
kenjiArai | 7:f244ea2ab994 | 239 | if (imu.chip_ready() == 0) { |
kenjiArai | 2:cf77282aea7b | 240 | do { |
kenjiArai | 7:f244ea2ab994 | 241 | printf("Bosch BNO055 is NOT avirable!!\r\n"); |
kenjiArai | 6:5f380fbcf849 | 242 | pwr_onoff = 0; // Power off |
kenjiArai | 7:f244ea2ab994 | 243 | ThisThread::sleep_for(100ms); |
kenjiArai | 6:5f380fbcf849 | 244 | pwr_onoff = 1; // Power on |
kenjiArai | 7:f244ea2ab994 | 245 | ThisThread::sleep_for(20ms); |
kenjiArai | 2:cf77282aea7b | 246 | } while(imu.reset()); |
kenjiArai | 0:31451519d283 | 247 | } |
kenjiArai | 0:31451519d283 | 248 | imu.set_mounting_position(MT_P6); |
kenjiArai | 7:f244ea2ab994 | 249 | printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n", |
kenjiArai | 7:f244ea2ab994 | 250 | imu.read_reg0(BNO055_AXIS_MAP_CONFIG), |
kenjiArai | 7:f244ea2ab994 | 251 | imu.read_reg0(BNO055_AXIS_MAP_SIGN) |
kenjiArai | 7:f244ea2ab994 | 252 | ); |
kenjiArai | 0:31451519d283 | 253 | imu.read_id_inf(&bno055_id_inf); |
kenjiArai | 7:f244ea2ab994 | 254 | printf("CHIP:0x%02x, ACC:0x%02x, MAG:0x%02x,", |
kenjiArai | 7:f244ea2ab994 | 255 | bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id |
kenjiArai | 7:f244ea2ab994 | 256 | ); |
kenjiArai | 7:f244ea2ab994 | 257 | printf("GYR:0x%02x, , SW:0x%04x, , BL:0x%02x\r\n", |
kenjiArai | 7:f244ea2ab994 | 258 | bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, |
kenjiArai | 7:f244ea2ab994 | 259 | bno055_id_inf.bootldr_rev_id |
kenjiArai | 7:f244ea2ab994 | 260 | ); |
kenjiArai | 7:f244ea2ab994 | 261 | while(true) { |
kenjiArai | 7:f244ea2ab994 | 262 | printf("Euler Angles data\r\n"); |
kenjiArai | 7:f244ea2ab994 | 263 | for (i = 0; i < NUM_LOOP; i++) { |
kenjiArai | 0:31451519d283 | 264 | imu.get_Euler_Angles(&euler_angles); |
kenjiArai | 7:f244ea2ab994 | 265 | printf("Heading:%+6.1f [deg], Roll:%+6.1f [deg],", |
kenjiArai | 7:f244ea2ab994 | 266 | euler_angles.h, euler_angles.r,); |
kenjiArai | 7:f244ea2ab994 | 267 | printf(" Pich:%+6.1f [deg], #%02d\r\n", |
kenjiArai | 7:f244ea2ab994 | 268 | euler_angles.p, i); |
kenjiArai | 7:f244ea2ab994 | 269 | ThisThread::sleep_for(500ms); |
kenjiArai | 0:31451519d283 | 270 | } |
kenjiArai | 7:f244ea2ab994 | 271 | printf("Quaternion data\r\n"); |
kenjiArai | 7:f244ea2ab994 | 272 | for (i = 0; i < NUM_LOOP; i++) { |
kenjiArai | 7:f244ea2ab994 | 273 | imu.get_quaternion(&quaternion); |
kenjiArai | 7:f244ea2ab994 | 274 | printf("W:%d, X:%d, Y:%d, Z:%d, #%02d\r\n", |
kenjiArai | 7:f244ea2ab994 | 275 | quaternion.w, quaternion.x, quaternion.y, quaternion.z, i); |
kenjiArai | 7:f244ea2ab994 | 276 | ThisThread::sleep_for(500ms); |
kenjiArai | 7:f244ea2ab994 | 277 | } |
kenjiArai | 7:f244ea2ab994 | 278 | printf("Linear accel data\r\n"); |
kenjiArai | 7:f244ea2ab994 | 279 | for (i = 0; i < NUM_LOOP; i++) { |
kenjiArai | 0:31451519d283 | 280 | imu.get_linear_accel(&linear_acc); |
kenjiArai | 7:f244ea2ab994 | 281 | printf( |
kenjiArai | 7:f244ea2ab994 | 282 | "X:%+6.1f[m/s*s], Y:%+6.1f[m/s*s], Z:%+6.1f[m/s*s], #%02d\r\n", |
kenjiArai | 7:f244ea2ab994 | 283 | linear_acc.x, linear_acc.y, linear_acc.z, i |
kenjiArai | 7:f244ea2ab994 | 284 | ); |
kenjiArai | 7:f244ea2ab994 | 285 | ThisThread::sleep_for(500ms); |
kenjiArai | 0:31451519d283 | 286 | } |
kenjiArai | 7:f244ea2ab994 | 287 | printf("Gravity vector data\r\n"); |
kenjiArai | 7:f244ea2ab994 | 288 | for (i = 0; i < NUM_LOOP; i++) { |
kenjiArai | 0:31451519d283 | 289 | imu.get_gravity(&gravity); |
kenjiArai | 7:f244ea2ab994 | 290 | printf( |
kenjiArai | 7:f244ea2ab994 | 291 | "X:%+6.1f[m/s*s], Y:%+6.1f[m/s*s], Z:%+6.1f[m/s*s], #%02d\r\n", |
kenjiArai | 7:f244ea2ab994 | 292 | gravity.x, gravity.y, gravity.z, i |
kenjiArai | 7:f244ea2ab994 | 293 | ); |
kenjiArai | 7:f244ea2ab994 | 294 | ThisThread::sleep_for(500ms); |
kenjiArai | 0:31451519d283 | 295 | } |
kenjiArai | 7:f244ea2ab994 | 296 | printf("Chip temperature data\r\n"); |
kenjiArai | 7:f244ea2ab994 | 297 | for (i = 0; i < (NUM_LOOP / 4); i++) { |
kenjiArai | 0:31451519d283 | 298 | imu.get_chip_temperature(&chip_temp); |
kenjiArai | 7:f244ea2ab994 | 299 | printf("Acc chip:%+d [degC], Gyr chip:%+d [degC], #%02d\r\n", |
kenjiArai | 7:f244ea2ab994 | 300 | chip_temp.acc_chip, chip_temp.gyr_chip, i); |
kenjiArai | 7:f244ea2ab994 | 301 | ThisThread::sleep_for(500ms); |
kenjiArai | 0:31451519d283 | 302 | } |
kenjiArai | 0:31451519d283 | 303 | } |
kenjiArai | 0:31451519d283 | 304 | } |
kenjiArai | 3:f5b5c4d795ce | 305 | #endif |