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@3:f5b5c4d795ce, 2015-04-08 (annotated)
- Committer:
- kenjiArai
- Date:
- Wed Apr 08 11:30:09 2015 +0000
- Revision:
- 3:f5b5c4d795ce
- Parent:
- 2:cf77282aea7b
- Child:
- 4:6d1118089a36
added different way of data output. Added to show calib. status
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 | 3:f5b5c4d795ce | 10 | * Revised: April 8th, 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 | 3:f5b5c4d795ce | 26 | #define FORMAT_0 0 |
kenjiArai | 3:f5b5c4d795ce | 27 | #define FORMAT_1 1 |
kenjiArai | 0:31451519d283 | 28 | |
kenjiArai | 0:31451519d283 | 29 | // Object ---------------------------------------------------------------------------------------- |
kenjiArai | 0:31451519d283 | 30 | Serial pc(USBTX,USBRX); |
kenjiArai | 2:cf77282aea7b | 31 | #if defined(TARGET_LPC1114) |
kenjiArai | 2:cf77282aea7b | 32 | DigitalOut pwr_onoff(dp17); |
kenjiArai | 2:cf77282aea7b | 33 | I2C i2c(dp5, dp27); // SDA, SCL |
kenjiArai | 2:cf77282aea7b | 34 | BNO055 imu(i2c, dp18); // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default |
kenjiArai | 2:cf77282aea7b | 35 | #elif defined(TARGET_LPC1768) |
kenjiArai | 2:cf77282aea7b | 36 | DigitalOut pwr_onoff(p30); |
kenjiArai | 2:cf77282aea7b | 37 | I2C i2c(p28, p27); // SDA, SCL |
kenjiArai | 2:cf77282aea7b | 38 | BNO055 imu(i2c, p29); // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default |
kenjiArai | 2:cf77282aea7b | 39 | #elif defined(TARGET_STM32L152RE) || defined(TARGET_STM32F401RE) || defined(TARGET_STM32F411RE) |
kenjiArai | 2:cf77282aea7b | 40 | DigitalOut pwr_onoff(PB_10); |
kenjiArai | 0:31451519d283 | 41 | I2C i2c(PB_9, PB_8); // SDA, SCL |
kenjiArai | 2:cf77282aea7b | 42 | BNO055 imu(i2c, PA_8); // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default |
kenjiArai | 2:cf77282aea7b | 43 | #elif defined(TARGET_RZ_A1H) |
kenjiArai | 2:cf77282aea7b | 44 | DigitalOut pwr_onoff(P8_11); |
kenjiArai | 2:cf77282aea7b | 45 | I2C i2c(P1_3, P1_2); // SDA, SCL |
kenjiArai | 2:cf77282aea7b | 46 | BNO055 imu(i2c, P8_13); // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default |
kenjiArai | 2:cf77282aea7b | 47 | #else |
kenjiArai | 2:cf77282aea7b | 48 | #error "Not cheched yet" |
kenjiArai | 2:cf77282aea7b | 49 | #endif |
kenjiArai | 3:f5b5c4d795ce | 50 | Timer t; |
kenjiArai | 0:31451519d283 | 51 | |
kenjiArai | 0:31451519d283 | 52 | // RAM ------------------------------------------------------------------------------------------- |
kenjiArai | 0:31451519d283 | 53 | BNO055_ID_INF_TypeDef bno055_id_inf; |
kenjiArai | 0:31451519d283 | 54 | BNO055_EULER_TypeDef euler_angles; |
kenjiArai | 0:31451519d283 | 55 | BNO055_QUATERNION_TypeDef quaternion; |
kenjiArai | 0:31451519d283 | 56 | BNO055_LIN_ACC_TypeDef linear_acc; |
kenjiArai | 0:31451519d283 | 57 | BNO055_GRAVITY_TypeDef gravity; |
kenjiArai | 0:31451519d283 | 58 | BNO055_TEMPERATURE_TypeDef chip_temp; |
kenjiArai | 0:31451519d283 | 59 | |
kenjiArai | 0:31451519d283 | 60 | // ROM / Constant data --------------------------------------------------------------------------- |
kenjiArai | 0:31451519d283 | 61 | |
kenjiArai | 0:31451519d283 | 62 | // Function prototypes --------------------------------------------------------------------------- |
kenjiArai | 0:31451519d283 | 63 | |
kenjiArai | 0:31451519d283 | 64 | //------------------------------------------------------------------------------------------------- |
kenjiArai | 0:31451519d283 | 65 | // Control Program |
kenjiArai | 3:f5b5c4d795ce | 66 | //------------------------------------------------------------------------------------------------- |
kenjiArai | 3:f5b5c4d795ce | 67 | #if FORMAT_0 |
kenjiArai | 3:f5b5c4d795ce | 68 | |
kenjiArai | 0:31451519d283 | 69 | int main() { |
kenjiArai | 0:31451519d283 | 70 | uint8_t i; |
kenjiArai | 0:31451519d283 | 71 | |
kenjiArai | 2:cf77282aea7b | 72 | pwr_onoff = 0; |
kenjiArai | 0:31451519d283 | 73 | pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n"); |
kenjiArai | 0:31451519d283 | 74 | // Is BNO055 avairable? |
kenjiArai | 0:31451519d283 | 75 | if (imu.chip_ready() == 0){ |
kenjiArai | 2:cf77282aea7b | 76 | do { |
kenjiArai | 2:cf77282aea7b | 77 | pc.printf("Bosch BNO055 is NOT avirable!!\r\n"); |
kenjiArai | 2:cf77282aea7b | 78 | pwr_onoff = 1; // Power off |
kenjiArai | 2:cf77282aea7b | 79 | wait(0.1); |
kenjiArai | 2:cf77282aea7b | 80 | pwr_onoff = 0; // Power on |
kenjiArai | 2:cf77282aea7b | 81 | wait(0.02); |
kenjiArai | 2:cf77282aea7b | 82 | } while(imu.reset()); |
kenjiArai | 0:31451519d283 | 83 | } |
kenjiArai | 0:31451519d283 | 84 | imu.set_mounting_position(MT_P6); |
kenjiArai | 0:31451519d283 | 85 | pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n", |
kenjiArai | 0:31451519d283 | 86 | imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN)); |
kenjiArai | 0:31451519d283 | 87 | imu.read_id_inf(&bno055_id_inf); |
kenjiArai | 0:31451519d283 | 88 | pc.printf("CHIP:0x%02x, ACC:0x%02x, MAG:0x%02x, GYR:0x%02x, , SW:0x%04x, , BL:0x%02x\r\n", |
kenjiArai | 0:31451519d283 | 89 | bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, |
kenjiArai | 0:31451519d283 | 90 | bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); |
kenjiArai | 0:31451519d283 | 91 | while(1) { |
kenjiArai | 0:31451519d283 | 92 | pc.printf("Euler Angles data\r\n"); |
kenjiArai | 2:cf77282aea7b | 93 | for (i = 0; i < NUM_LOOP; i++){ |
kenjiArai | 0:31451519d283 | 94 | imu.get_Euler_Angles(&euler_angles); |
kenjiArai | 0:31451519d283 | 95 | pc.printf("Heading:%+6.1f [deg], Roll:%+6.1f [deg], Pich:%+6.1f [deg], #%02d\r\n", |
kenjiArai | 0:31451519d283 | 96 | euler_angles.h, euler_angles.r, euler_angles.p, i); |
kenjiArai | 0:31451519d283 | 97 | wait(0.5); |
kenjiArai | 0:31451519d283 | 98 | } |
kenjiArai | 0:31451519d283 | 99 | pc.printf("Quaternion data\r\n"); |
kenjiArai | 2:cf77282aea7b | 100 | for (i = 0; i < NUM_LOOP; i++){ |
kenjiArai | 0:31451519d283 | 101 | imu.get_quaternion(&quaternion); |
kenjiArai | 0:31451519d283 | 102 | pc.printf("W:%d, X:%d, Y:%d, Z:%d, #%02d\r\n", |
kenjiArai | 0:31451519d283 | 103 | quaternion.w, quaternion.x, quaternion.y, quaternion.z, i); |
kenjiArai | 0:31451519d283 | 104 | wait(0.5); |
kenjiArai | 0:31451519d283 | 105 | } |
kenjiArai | 0:31451519d283 | 106 | pc.printf("Linear accel data\r\n"); |
kenjiArai | 2:cf77282aea7b | 107 | for (i = 0; i < NUM_LOOP; i++){ |
kenjiArai | 0:31451519d283 | 108 | imu.get_linear_accel(&linear_acc); |
kenjiArai | 0:31451519d283 | 109 | pc.printf("X:%+6.1f [m/s*s], Y:%+6.1f [m/s*s], Z:%+6.1f [m/s*s], #%02d\r\n", |
kenjiArai | 0:31451519d283 | 110 | linear_acc.x, linear_acc.y, linear_acc.z, i); |
kenjiArai | 0:31451519d283 | 111 | wait(0.5); |
kenjiArai | 0:31451519d283 | 112 | } |
kenjiArai | 0:31451519d283 | 113 | pc.printf("Gravity vector data\r\n"); |
kenjiArai | 2:cf77282aea7b | 114 | for (i = 0; i < NUM_LOOP; i++){ |
kenjiArai | 0:31451519d283 | 115 | imu.get_gravity(&gravity); |
kenjiArai | 0:31451519d283 | 116 | pc.printf("X:%+6.1f [m/s*s], Y:%+6.1f [m/s*s], Z:%+6.1f [m/s*s], #%02d\r\n", |
kenjiArai | 0:31451519d283 | 117 | gravity.x, gravity.y, gravity.z, i); |
kenjiArai | 0:31451519d283 | 118 | wait(0.5); |
kenjiArai | 0:31451519d283 | 119 | } |
kenjiArai | 0:31451519d283 | 120 | pc.printf("Chip temperature data\r\n"); |
kenjiArai | 2:cf77282aea7b | 121 | for (i = 0; i < (NUM_LOOP / 4); i++){ |
kenjiArai | 0:31451519d283 | 122 | imu.get_chip_temperature(&chip_temp); |
kenjiArai | 0:31451519d283 | 123 | pc.printf("Acc chip:%+d [degC], Gyr chip:%+d [degC], #%02d\r\n", |
kenjiArai | 0:31451519d283 | 124 | chip_temp.acc_chip, chip_temp.gyr_chip, i); |
kenjiArai | 0:31451519d283 | 125 | wait(0.5); |
kenjiArai | 0:31451519d283 | 126 | } |
kenjiArai | 0:31451519d283 | 127 | } |
kenjiArai | 0:31451519d283 | 128 | } |
kenjiArai | 3:f5b5c4d795ce | 129 | |
kenjiArai | 3:f5b5c4d795ce | 130 | #elif FORMAT_1 |
kenjiArai | 3:f5b5c4d795ce | 131 | |
kenjiArai | 3:f5b5c4d795ce | 132 | int main() { |
kenjiArai | 3:f5b5c4d795ce | 133 | pwr_onoff = 0; |
kenjiArai | 3:f5b5c4d795ce | 134 | pc.printf("\r\n\r\nIf pc terminal soft is ready, please hit any key!\r\n"); |
kenjiArai | 3:f5b5c4d795ce | 135 | char c = pc.getc(); |
kenjiArai | 3:f5b5c4d795ce | 136 | pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n"); |
kenjiArai | 3:f5b5c4d795ce | 137 | // Is BNO055 avairable? |
kenjiArai | 3:f5b5c4d795ce | 138 | if (imu.chip_ready() == 0){ |
kenjiArai | 3:f5b5c4d795ce | 139 | do { |
kenjiArai | 3:f5b5c4d795ce | 140 | pc.printf("Bosch BNO055 is NOT avirable!!\r\n"); |
kenjiArai | 3:f5b5c4d795ce | 141 | pwr_onoff = 1; // Power off |
kenjiArai | 3:f5b5c4d795ce | 142 | wait(0.1); |
kenjiArai | 3:f5b5c4d795ce | 143 | pwr_onoff = 0; // Power on |
kenjiArai | 3:f5b5c4d795ce | 144 | wait(0.02); |
kenjiArai | 3:f5b5c4d795ce | 145 | } while(imu.reset()); |
kenjiArai | 3:f5b5c4d795ce | 146 | } |
kenjiArai | 3:f5b5c4d795ce | 147 | imu.set_mounting_position(MT_P6); |
kenjiArai | 3:f5b5c4d795ce | 148 | pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n", |
kenjiArai | 3:f5b5c4d795ce | 149 | imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN)); |
kenjiArai | 3:f5b5c4d795ce | 150 | imu.read_id_inf(&bno055_id_inf); |
kenjiArai | 3:f5b5c4d795ce | 151 | pc.printf("CHIP:0x%02x, ACC:0x%02x, MAG:0x%02x, GYR:0x%02x, , SW:0x%04x, , BL:0x%02x\r\n", |
kenjiArai | 3:f5b5c4d795ce | 152 | bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, |
kenjiArai | 3:f5b5c4d795ce | 153 | bno055_id_inf.gyr_id, bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id); |
kenjiArai | 3:f5b5c4d795ce | 154 | pc.printf("[E]:Euler Angles[deg],[Q]:Quaternion[],[L]:Linear accel[m/s*s],"); |
kenjiArai | 3:f5b5c4d795ce | 155 | pc.printf("[G]:Gravity vector[m/s*s],[T]:Chip temperature,Acc,Gyr[degC],[S]:Status,[M]:time[mS]\r\n"); |
kenjiArai | 3:f5b5c4d795ce | 156 | t.start(); |
kenjiArai | 3:f5b5c4d795ce | 157 | while(1) { |
kenjiArai | 3:f5b5c4d795ce | 158 | imu.get_Euler_Angles(&euler_angles); |
kenjiArai | 3:f5b5c4d795ce | 159 | pc.printf("[E],Y,%+6.1f,R,%+6.1f,P,%+6.1f,", |
kenjiArai | 3:f5b5c4d795ce | 160 | euler_angles.h, euler_angles.r, euler_angles.p); |
kenjiArai | 3:f5b5c4d795ce | 161 | imu.get_quaternion(&quaternion); |
kenjiArai | 3:f5b5c4d795ce | 162 | pc.printf("[Q],W:%d,X:%d,Y:%d,Z:%d,", |
kenjiArai | 3:f5b5c4d795ce | 163 | quaternion.w, quaternion.x, quaternion.y, quaternion.z); |
kenjiArai | 3:f5b5c4d795ce | 164 | imu.get_linear_accel(&linear_acc); |
kenjiArai | 3:f5b5c4d795ce | 165 | pc.printf("[L],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", |
kenjiArai | 3:f5b5c4d795ce | 166 | linear_acc.x, linear_acc.y, linear_acc.z); |
kenjiArai | 3:f5b5c4d795ce | 167 | imu.get_gravity(&gravity); |
kenjiArai | 3:f5b5c4d795ce | 168 | pc.printf("[G],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,", |
kenjiArai | 3:f5b5c4d795ce | 169 | gravity.x, gravity.y, gravity.z); |
kenjiArai | 3:f5b5c4d795ce | 170 | imu.get_chip_temperature(&chip_temp); |
kenjiArai | 3:f5b5c4d795ce | 171 | pc.printf("[T],%+d,%+d,", |
kenjiArai | 3:f5b5c4d795ce | 172 | chip_temp.acc_chip, chip_temp.gyr_chip); |
kenjiArai | 3:f5b5c4d795ce | 173 | pc.printf("[S],0x%x,[M],%d\r\n", |
kenjiArai | 3:f5b5c4d795ce | 174 | imu.read_calib_status(), t.read_ms()); |
kenjiArai | 3:f5b5c4d795ce | 175 | } |
kenjiArai | 3:f5b5c4d795ce | 176 | } |
kenjiArai | 3:f5b5c4d795ce | 177 | |
kenjiArai | 3:f5b5c4d795ce | 178 | #endif |