fsdf
Dependencies: BNO055_fusion MODSERIAL mbed
Fork of Shared-1BNO055 by
main.cpp@6:0590c7ff8c34, 2016-01-24 (annotated)
- Committer:
- tommyallen
- Date:
- Sun Jan 24 17:04:40 2016 +0000
- Revision:
- 6:0590c7ff8c34
- Parent:
- 5:9594519c9462
- Child:
- 7:d4b5e83c7947
Modified: https://developer.mbed.org/users/kenjiArai/code/Bosch_BNO055_Fusion_example/; ; TCAllen UoL UK
Who changed what in which revision?
User | Revision | Line number | New 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); |
tommyallen | 6:0590c7ff8c34 | 28 | |
kenjiArai | 2:cf77282aea7b | 29 | DigitalOut pwr_onoff(p30); |
kenjiArai | 2:cf77282aea7b | 30 | I2C i2c(p28, p27); // SDA, SCL |
kenjiArai | 2:cf77282aea7b | 31 | BNO055 imu(i2c, p29); // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default |
tommyallen | 6:0590c7ff8c34 | 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; |
kenjiArai | 3:f5b5c4d795ce | 54 | |
tommyallen | 6:0590c7ff8c34 | 55 | pc.printf("calibrating:, ------ 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(); |
tommyallen | 6:0590c7ff8c34 | 63 | pc.printf("calibrating = 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(); |
tommyallen | 6:0590c7ff8c34 | 79 | pc.printf("calibrating, = 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); |
tommyallen | 6:0590c7ff8c34 | 101 | pc.printf("calibrating = 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 | } |
tommyallen | 6:0590c7ff8c34 | 114 | //-------------------------------------------------------------------------------- |
kenjiArai | 4:6d1118089a36 | 115 | int main(){ |
tommyallen | 6:0590c7ff8c34 | 116 | pc.baud(9600); |
tommyallen | 6:0590c7ff8c34 | 117 | pc.printf("notstreaming: BNO055 Hello World\r\n\r\n"); |
kenjiArai | 4:6d1118089a36 | 118 | imu.set_mounting_position(MT_P6); |
kenjiArai | 4:6d1118089a36 | 119 | pwr_onoff = 0; |
tommyallen | 6:0590c7ff8c34 | 120 | pc.printf(",\r\n\r\nIf pc terminal soft is ready, please hit any key!\r\n"); |
kenjiArai | 4:6d1118089a36 | 121 | char c = pc.getc(); |
tommyallen | 6:0590c7ff8c34 | 122 | pc.printf(",Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n"); |
kenjiArai | 4:6d1118089a36 | 123 | // Is BNO055 avairable? |
kenjiArai | 4:6d1118089a36 | 124 | if (imu.chip_ready() == 0){ |
kenjiArai | 4:6d1118089a36 | 125 | do { |
tommyallen | 6:0590c7ff8c34 | 126 | pc.printf(",Bosch BNO055 is NOT avirable!!\r\n Reset\r\n"); |
kenjiArai | 4:6d1118089a36 | 127 | pwr_onoff = 1; // Power off |
kenjiArai | 4:6d1118089a36 | 128 | wait(0.1); |
kenjiArai | 4:6d1118089a36 | 129 | pwr_onoff = 0; // Power on |
kenjiArai | 4:6d1118089a36 | 130 | wait(0.02); |
kenjiArai | 4:6d1118089a36 | 131 | } while(imu.reset()); |
kenjiArai | 4:6d1118089a36 | 132 | } |
kenjiArai | 4:6d1118089a36 | 133 | pc.printf("Bosch BNO055 is available now!!\r\n"); |
kenjiArai | 4:6d1118089a36 | 134 | pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n", |
kenjiArai | 4:6d1118089a36 | 135 | imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN)); |
kenjiArai | 4:6d1118089a36 | 136 | imu.read_id_inf(&bno055_id_inf); |
kenjiArai | 4:6d1118089a36 | 137 | pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x, ", |
kenjiArai | 4:6d1118089a36 | 138 | bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id); |
kenjiArai | 4:6d1118089a36 | 139 | pc.printf("SW REV:0x%04x, BL REV:0x%02x\r\n", |
kenjiArai | 4:6d1118089a36 | 140 | bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); |
kenjiArai | 4:6d1118089a36 | 141 | pc.printf("If you would like to calibrate the BNO055, please hit 'y' (No: any other key)\r\n"); |
kenjiArai | 4:6d1118089a36 | 142 | c = pc.getc(); |
kenjiArai | 4:6d1118089a36 | 143 | if (c == 'y'){ |
kenjiArai | 4:6d1118089a36 | 144 | bno055_calbration(); |
kenjiArai | 4:6d1118089a36 | 145 | } |
kenjiArai | 4:6d1118089a36 | 146 | pc.printf("[E]:Euler Angles[deg],[Q]:Quaternion[],[L]:Linear accel[m/s*s],"); |
kenjiArai | 4:6d1118089a36 | 147 | pc.printf("[G]:Gravity vector[m/s*s],[T]:Chip temperature,Acc,Gyr[degC],[S]:Status,[M]:time[mS]\r\n"); |
kenjiArai | 4:6d1118089a36 | 148 | t.start(); |
tommyallen | 6:0590c7ff8c34 | 149 | |
tommyallen | 6:0590c7ff8c34 | 150 | //Set Register into Config Mode |
tommyallen | 6:0590c7ff8c34 | 151 | //imu.write_reg0(0x3D, 0x00); |
tommyallen | 6:0590c7ff8c34 | 152 | //Set register unit to Radians |
tommyallen | 6:0590c7ff8c34 | 153 | imu.write_reg0(0x3B, 0x02); |
tommyallen | 6:0590c7ff8c34 | 154 | //Change back into normal operation mode |
tommyallen | 6:0590c7ff8c34 | 155 | //imu.write_reg0(0x3D, 0x0C); |
tommyallen | 6:0590c7ff8c34 | 156 | |
tommyallen | 6:0590c7ff8c34 | 157 | while(1) { |
kenjiArai | 4:6d1118089a36 | 158 | imu.get_Euler_Angles(&euler_angles); |
tommyallen | 6:0590c7ff8c34 | 159 | pc.printf("Streaming:,Orientation:,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", |
tommyallen | 6:0590c7ff8c34 | 160 | euler_angles.h, euler_angles.r, euler_angles.p); |
kenjiArai | 4:6d1118089a36 | 161 | imu.get_linear_accel(&linear_acc); |
tommyallen | 6:0590c7ff8c34 | 162 | pc.printf("Acc:,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", |
tommyallen | 6:0590c7ff8c34 | 163 | linear_acc.x, linear_acc.y, linear_acc.z); |
tommyallen | 6:0590c7ff8c34 | 164 | |
kenjiArai | 4:6d1118089a36 | 165 | imu.get_gravity(&gravity); |
tommyallen | 6:0590c7ff8c34 | 166 | pc.printf("Gravity,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", |
kenjiArai | 4:6d1118089a36 | 167 | gravity.x, gravity.y, gravity.z); |
tommyallen | 6:0590c7ff8c34 | 168 | |
tommyallen | 6:0590c7ff8c34 | 169 | pc.printf("Time:,%d,\n",t.read_ms()); |
tommyallen | 6:0590c7ff8c34 | 170 | |
tommyallen | 6:0590c7ff8c34 | 171 | |
tommyallen | 6:0590c7ff8c34 | 172 | |
tommyallen | 6:0590c7ff8c34 | 173 | // c = pc.getc(); |
tommyallen | 6:0590c7ff8c34 | 174 | // if (c == 'c'){ |
tommyallen | 6:0590c7ff8c34 | 175 | // bno055_calbration(); |
tommyallen | 6:0590c7ff8c34 | 176 | // } |
tommyallen | 6:0590c7ff8c34 | 177 | // if (c == 'r'){ |
tommyallen | 6:0590c7ff8c34 | 178 | // imu.reset(); |
tommyallen | 6:0590c7ff8c34 | 179 | // } |
kenjiArai | 4:6d1118089a36 | 180 | } |
kenjiArai | 4:6d1118089a36 | 181 | } |
kenjiArai | 4:6d1118089a36 | 182 | |
tommyallen | 6:0590c7ff8c34 | 183 | // imu.get_Euler_Angles(&euler_angles); |
tommyallen | 6:0590c7ff8c34 | 184 | // pc.printf("[E],Y,%+6.1f,R,%+6.1f,P,%+6.1f,", |
tommyallen | 6:0590c7ff8c34 | 185 | // euler_angles.h, euler_angles.r, euler_angles.p); |
tommyallen | 6:0590c7ff8c34 | 186 | // imu.get_quaternion(&quaternion); |
tommyallen | 6:0590c7ff8c34 | 187 | // pc.printf("[Q],W,%d,X,%d,Y,%d,Z,%d,", |
tommyallen | 6:0590c7ff8c34 | 188 | // quaternion.w, quaternion.x, quaternion.y, quaternion.z); |
tommyallen | 6:0590c7ff8c34 | 189 | // imu.get_linear_accel(&linear_acc); |
tommyallen | 6:0590c7ff8c34 | 190 | // pc.printf("[L],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", |
tommyallen | 6:0590c7ff8c34 | 191 | // linear_acc.x, linear_acc.y, linear_acc.z); |
tommyallen | 6:0590c7ff8c34 | 192 | // imu.get_gravity(&gravity); |
tommyallen | 6:0590c7ff8c34 | 193 | // pc.printf("[G],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", |
tommyallen | 6:0590c7ff8c34 | 194 | // gravity.x, gravity.y, gravity.z); |
tommyallen | 6:0590c7ff8c34 | 195 | // imu.get_chip_temperature(&chip_temp); |
tommyallen | 6:0590c7ff8c34 | 196 | // pc.printf("[T],%+d,%+d,", |
tommyallen | 6:0590c7ff8c34 | 197 | // chip_temp.acc_chip, chip_temp.gyr_chip); |
tommyallen | 6:0590c7ff8c34 | 198 | // pc.printf("[S],0x%x,[M],%d\r\n", |
tommyallen | 6:0590c7ff8c34 | 199 | // imu.read_calib_status(), t.read_ms()); |
kenjiArai | 0:31451519d283 | 200 | |
tommyallen | 6:0590c7ff8c34 | 201 | // pc.printf("Words\r\n"); |