Thomas Allen / Mbed 2 deprecated BNO055-ELEC3810

v1.0

Dependencies:   BNO055_fusion mbed MODSERIAL dsp

Fork of Bosch_BNO055_Fusion_example by Kenji Arai

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?

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 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