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@10:acbb851070c2, 2016-02-07 (annotated)

Committer:
tommyallen
Date:
Sun Feb 07 22:21:26 2016 +0000
Revision:
10:acbb851070c2
Parent:
9:7be40e42e578 
07/02/16;

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"
tommyallen 7:d4b5e83c7947 22 #include "MODSERIAL.h"
tommyallen 9:7be40e42e578 23 #include "arm_math.h"
kenjiArai 0:31451519d283 24
kenjiArai 0:31451519d283 25 // Definition ------------------------------------------------------------------------------------
kenjiArai 3:f5b5c4d795ce 26 #define NUM_LOOP 100
kenjiArai 3:f5b5c4d795ce 27
kenjiArai 0:31451519d283 28 // Object ----------------------------------------------------------------------------------------
tommyallen 7:d4b5e83c7947 29 MODSERIAL pc(USBTX,USBRX);
tommyallen 7:d4b5e83c7947 30
tommyallen 8:41d1f9ab3be0 31 volatile bool RAW = true;
tommyallen 8:41d1f9ab3be0 32 volatile bool FUS = false;
tommyallen 7:d4b5e83c7947 33 bool printed = false;
tommyallen 9:7be40e42e578 34 int Time;
tommyallen 10:acbb851070c2 35 int timeStamp = 0;
tommyallen 10:acbb851070c2 36 int timeOfStart;
tommyallen 10:acbb851070c2 37 int timeNow;
tommyallen 6:0590c7ff8c34 38
tommyallen 8:41d1f9ab3be0 39 //string sdata = "";
tommyallen 8:41d1f9ab3be0 40
tommyallen 7:d4b5e83c7947 41 DigitalOut led1(LED1);
tommyallen 7:d4b5e83c7947 42 DigitalOut led2(LED2);
tommyallen 7:d4b5e83c7947 43 DigitalOut led3(LED3);
tommyallen 7:d4b5e83c7947 44 DigitalOut led4(LED4);
kenjiArai 2:cf77282aea7b 45 DigitalOut pwr_onoff(p30);
tommyallen 7:d4b5e83c7947 46
kenjiArai 2:cf77282aea7b 47 I2C i2c(p28, p27); // SDA, SCL
kenjiArai 2:cf77282aea7b 48 BNO055 imu(i2c, p29); // Reset =D7, addr = BNO055_G_CHIP_ADDR, mode = MODE_NDOF <- as default
tommyallen 7:d4b5e83c7947 49 LocalFileSystem local("local"); // Create the local filesystem under the name "local"
tommyallen 6:0590c7ff8c34 50
kenjiArai 3:f5b5c4d795ce 51 Timer t;
kenjiArai 0:31451519d283 52
kenjiArai 0:31451519d283 53 // RAM -------------------------------------------------------------------------------------------
kenjiArai 0:31451519d283 54 BNO055_ID_INF_TypeDef bno055_id_inf;
kenjiArai 0:31451519d283 55 BNO055_EULER_TypeDef euler_angles;
kenjiArai 0:31451519d283 56 BNO055_QUATERNION_TypeDef quaternion;
kenjiArai 0:31451519d283 57 BNO055_LIN_ACC_TypeDef linear_acc;
kenjiArai 0:31451519d283 58 BNO055_GRAVITY_TypeDef gravity;
kenjiArai 0:31451519d283 59 BNO055_TEMPERATURE_TypeDef chip_temp;
kenjiArai 0:31451519d283 60
kenjiArai 0:31451519d283 61 // ROM / Constant data ---------------------------------------------------------------------------
kenjiArai 0:31451519d283 62
kenjiArai 0:31451519d283 63 // Function prototypes ---------------------------------------------------------------------------
kenjiArai 0:31451519d283 64
tommyallen 7:d4b5e83c7947 65
tommyallen 7:d4b5e83c7947 66 //-------------------------------------------------------------------------------------------------
tommyallen 7:d4b5e83c7947 67 // Serial Interrupt
tommyallen 7:d4b5e83c7947 68 //-------------------------------------------------------------------------------------------------
tommyallen 7:d4b5e83c7947 69 void rxCallback(MODSERIAL_IRQ_INFO *q)
tommyallen 7:d4b5e83c7947 70 {
tommyallen 7:d4b5e83c7947 71 MODSERIAL *serial = q->serial;
tommyallen 8:41d1f9ab3be0 72 if ( serial->rxGetLastChar() == 'f') {
tommyallen 8:41d1f9ab3be0 73 imu.write_reg0(0x3d, 0x0c);
tommyallen 8:41d1f9ab3be0 74 RAW = false;
tommyallen 8:41d1f9ab3be0 75 FUS = true;
tommyallen 8:41d1f9ab3be0 76 pc.printf("\r\nSWITCHING TO FUSION DATA!\r\n");
tommyallen 8:41d1f9ab3be0 77 }
tommyallen 8:41d1f9ab3be0 78 if ( serial->rxGetLastChar() == 'r') {
tommyallen 8:41d1f9ab3be0 79 imu.write_reg0(0x3d, 0x07);
tommyallen 8:41d1f9ab3be0 80 RAW = true;
tommyallen 8:41d1f9ab3be0 81 FUS = false;
tommyallen 8:41d1f9ab3be0 82 pc.printf("\r\nSWITCHING TO RAW DATA!\r\n");
tommyallen 7:d4b5e83c7947 83 }
tommyallen 10:acbb851070c2 84 if ( serial->rxGetLastChar() == '*') {
tommyallen 10:acbb851070c2 85 timeOfStart = t.read_ms();
tommyallen 10:acbb851070c2 86 timeStamp = 0;
tommyallen 10:acbb851070c2 87 }
tommyallen 7:d4b5e83c7947 88 }
tommyallen 7:d4b5e83c7947 89
kenjiArai 0:31451519d283 90 //-------------------------------------------------------------------------------------------------
kenjiArai 0:31451519d283 91 // Control Program
kenjiArai 3:f5b5c4d795ce 92 //-------------------------------------------------------------------------------------------------
kenjiArai 4:6d1118089a36 93 // Calibration
kenjiArai 4:6d1118089a36 94 // Please refer BNO055 Data sheet 3.10 Calibration & 3.6.4 Sensor calibration data
tommyallen 7:d4b5e83c7947 95 void bno055_calbration(void)
tommyallen 7:d4b5e83c7947 96 {
kenjiArai 4:6d1118089a36 97 uint8_t d;
tommyallen 7:d4b5e83c7947 98 pc.printf("------ Enter BNO055 Manual Calibration Mode ------\r\n");
kenjiArai 4:6d1118089a36 99 //---------- Gyroscope Caliblation ------------------------------------------------------------
kenjiArai 4:6d1118089a36 100 // (a) Place the device in a single stable position for a period of few seconds to allow the
kenjiArai 4:6d1118089a36 101 // gyroscope to calibrate
kenjiArai 4:6d1118089a36 102 pc.printf("Step1) Please wait few seconds\r\n");
kenjiArai 4:6d1118089a36 103 t.start();
tommyallen 7:d4b5e83c7947 104 while (t.read() < 10) {
kenjiArai 4:6d1118089a36 105 d = imu.read_calib_status();
tommyallen 6:0590c7ff8c34 106 pc.printf("calibrating = 0x%x target = 0x30(at least)\r\n", d);
tommyallen 7:d4b5e83c7947 107 if ((d & 0x30) == 0x30) {
kenjiArai 4:6d1118089a36 108 break;
kenjiArai 4:6d1118089a36 109 }
kenjiArai 4:6d1118089a36 110 wait(1.0);
kenjiArai 4:6d1118089a36 111 }
kenjiArai 4:6d1118089a36 112 pc.printf("-> Step1) is done\r\n\r\n");
kenjiArai 4:6d1118089a36 113 //---------- Magnetometer Caliblation ---------------------------------------------------------
kenjiArai 4:6d1118089a36 114 // (a) Make some random movements (for example: writing the number ‘8’ on air) until the
kenjiArai 4:6d1118089a36 115 // CALIB_STAT register indicates fully calibrated.
kenjiArai 4:6d1118089a36 116 // (b) It takes more calibration movements to get the magnetometer calibrated than in the
kenjiArai 4:6d1118089a36 117 // NDOF mode.
kenjiArai 4:6d1118089a36 118 pc.printf("Step2) random moving (try to change the BNO055 axis)\r\n");
kenjiArai 4:6d1118089a36 119 t.start();
tommyallen 7:d4b5e83c7947 120 while (t.read() < 30) {
kenjiArai 4:6d1118089a36 121 d = imu.read_calib_status();
tommyallen 6:0590c7ff8c34 122 pc.printf("calibrating, = 0x%x target = 0x33(at least)\r\n", d);
tommyallen 7:d4b5e83c7947 123 if ((d & 0x03) == 0x03) {
kenjiArai 4:6d1118089a36 124 break;
kenjiArai 4:6d1118089a36 125 }
tommyallen 7:d4b5e83c7947 126 wait(1.0);
kenjiArai 4:6d1118089a36 127 }
kenjiArai 4:6d1118089a36 128 pc.printf("-> Step2) is done\r\n\r\n");
kenjiArai 4:6d1118089a36 129 //---------- Magnetometer Caliblation ---------------------------------------------------------
kenjiArai 4:6d1118089a36 130 // a) Place the device in 6 different stable positions for a period of few seconds
kenjiArai 4:6d1118089a36 131 // to allow the accelerometer to calibrate.
kenjiArai 4:6d1118089a36 132 // b) Make sure that there is slow movement between 2 stable positions
kenjiArai 4:6d1118089a36 133 // The 6 stable positions could be in any direction, but make sure that the device is
kenjiArai 4:6d1118089a36 134 // lying at least once perpendicular to the x, y and z axis.
kenjiArai 4:6d1118089a36 135 pc.printf("Step3) Change rotation each X,Y,Z axis KEEP SLOWLY!!");
kenjiArai 4:6d1118089a36 136 pc.printf(" Each 90deg stay a 5 sec and set at least 6 position.\r\n");
kenjiArai 4:6d1118089a36 137 pc.printf(" e.g. (1)ACC:X0,Y0,Z-9,(2)ACC:X9,Y0,Z0,(3)ACC:X0,Y0,Z9,");
kenjiArai 4:6d1118089a36 138 pc.printf("(4)ACC:X-9,Y0,Z0,(5)ACC:X0,Y-9,Z0,(6)ACC:X0,Y9,Z0,\r\n");
kenjiArai 4:6d1118089a36 139 pc.printf(" If you will give up, hit any key.\r\n", d);
kenjiArai 4:6d1118089a36 140 t.stop();
tommyallen 7:d4b5e83c7947 141 while (true) {
kenjiArai 4:6d1118089a36 142 d = imu.read_calib_status();
kenjiArai 4:6d1118089a36 143 imu.get_gravity(&gravity);
tommyallen 6:0590c7ff8c34 144 pc.printf("calibrating = 0x%x target = 0xff ACC:X %4.1f, Y %4.1f, Z %4.1f\r\n",
tommyallen 7:d4b5e83c7947 145 d, gravity.x, gravity.y, gravity.z);
tommyallen 7:d4b5e83c7947 146 if (d == 0xff) {
tommyallen 7:d4b5e83c7947 147 break;
tommyallen 7:d4b5e83c7947 148 }
tommyallen 7:d4b5e83c7947 149 if (pc.readable()) {
tommyallen 7:d4b5e83c7947 150 break;
tommyallen 7:d4b5e83c7947 151 }
kenjiArai 4:6d1118089a36 152 wait(1.0);
kenjiArai 4:6d1118089a36 153 }
tommyallen 7:d4b5e83c7947 154 if (imu.read_calib_status() == 0xff) {
kenjiArai 4:6d1118089a36 155 pc.printf("-> All of Calibration steps are done successfully!\r\n\r\n");
kenjiArai 4:6d1118089a36 156 } else {
kenjiArai 4:6d1118089a36 157 pc.printf("-> Calibration steps are suspended!\r\n\r\n");
kenjiArai 4:6d1118089a36 158 }
kenjiArai 4:6d1118089a36 159 t.stop();
kenjiArai 4:6d1118089a36 160 }
tommyallen 7:d4b5e83c7947 161
tommyallen 8:41d1f9ab3be0 162 //-------------------------------------------------------------------------------------------------
tommyallen 8:41d1f9ab3be0 163 //-------------------------------------------------------------------------------------------------
tommyallen 8:41d1f9ab3be0 164 //-------------------------------------------------------------------------------------------------
tommyallen 8:41d1f9ab3be0 165 //-------------------------------------------------------------------------------------------------
tommyallen 8:41d1f9ab3be0 166 // Main Program
tommyallen 8:41d1f9ab3be0 167 //-------------------------------------------------------------------------------------------------
tommyallen 8:41d1f9ab3be0 168 //-------------------------------------------------------------------------------------------------
tommyallen 8:41d1f9ab3be0 169 //-------------------------------------------------------------------------------------------------
tommyallen 8:41d1f9ab3be0 170 //-------------------------------------------------------------------------------------------------
tommyallen 7:d4b5e83c7947 171 int main()
tommyallen 7:d4b5e83c7947 172 {
tommyallen 8:41d1f9ab3be0 173 pc.baud(115200);
tommyallen 7:d4b5e83c7947 174 pc.attach(&rxCallback, MODSERIAL::RxIrq);
tommyallen 7:d4b5e83c7947 175 pc.printf("BNO055 Hello World\r\n\r\n");
kenjiArai 4:6d1118089a36 176 imu.set_mounting_position(MT_P6);
kenjiArai 4:6d1118089a36 177 pwr_onoff = 0;
tommyallen 7:d4b5e83c7947 178 pc.printf("\r\n\r\nIf pc terminal soft is ready, please hit any key!\r\n");
kenjiArai 4:6d1118089a36 179 char c = pc.getc();
tommyallen 7:d4b5e83c7947 180 pc.printf("Bosch Sensortec BNO055 test program on " __DATE__ "/" __TIME__ "\r\n");
kenjiArai 4:6d1118089a36 181 // Is BNO055 avairable?
tommyallen 7:d4b5e83c7947 182 if (imu.chip_ready() == 0) {
kenjiArai 4:6d1118089a36 183 do {
tommyallen 7:d4b5e83c7947 184 pc.printf("Bosch BNO055 is NOT avirable!!\r\n Reset\r\n");
kenjiArai 4:6d1118089a36 185 pwr_onoff = 1; // Power off
kenjiArai 4:6d1118089a36 186 wait(0.1);
kenjiArai 4:6d1118089a36 187 pwr_onoff = 0; // Power on
kenjiArai 4:6d1118089a36 188 wait(0.02);
kenjiArai 4:6d1118089a36 189 } while(imu.reset());
kenjiArai 4:6d1118089a36 190 }
kenjiArai 4:6d1118089a36 191 pc.printf("Bosch BNO055 is available now!!\r\n");
kenjiArai 4:6d1118089a36 192 pc.printf("AXIS_REMAP_CONFIG:0x%02x, AXIS_REMAP_SIGN:0x%02x\r\n",
tommyallen 7:d4b5e83c7947 193 imu.read_reg0(BNO055_AXIS_MAP_CONFIG), imu.read_reg0(BNO055_AXIS_MAP_SIGN));
kenjiArai 4:6d1118089a36 194 imu.read_id_inf(&bno055_id_inf);
kenjiArai 4:6d1118089a36 195 pc.printf("CHIP ID:0x%02x, ACC ID:0x%02x, MAG ID:0x%02x, GYR ID:0x%02x, ",
tommyallen 7:d4b5e83c7947 196 bno055_id_inf.chip_id, bno055_id_inf.acc_id, bno055_id_inf.mag_id, bno055_id_inf.gyr_id);
kenjiArai 4:6d1118089a36 197 pc.printf("SW REV:0x%04x, BL REV:0x%02x\r\n",
tommyallen 7:d4b5e83c7947 198 bno055_id_inf.sw_rev_id, bno055_id_inf.bootldr_rev_id);
kenjiArai 4:6d1118089a36 199 pc.printf("If you would like to calibrate the BNO055, please hit 'y' (No: any other key)\r\n");
kenjiArai 4:6d1118089a36 200 c = pc.getc();
tommyallen 7:d4b5e83c7947 201 if (c == 'y') {
kenjiArai 4:6d1118089a36 202 bno055_calbration();
kenjiArai 4:6d1118089a36 203 }
kenjiArai 4:6d1118089a36 204 t.start();
tommyallen 8:41d1f9ab3be0 205 imu.write_reg0(0x3d, 0x07); // Default FUS
tommyallen 7:d4b5e83c7947 206
tommyallen 8:41d1f9ab3be0 207 while (1) {
tommyallen 10:acbb851070c2 208 while (RAW == true) {
tommyallen 9:7be40e42e578 209 Time = t.read_ms();
tommyallen 10:acbb851070c2 210 timeNow = Time + timeOfStart;
tommyallen 10:acbb851070c2 211 if (timeNow > timeStamp + 9) {
tommyallen 9:7be40e42e578 212 pc.printf("RAW,xLSB:,%d,xMSB:,%d,yLSB:%d,yMSB:,%d,zLSB:,%d,zMSB:,%d,"
tommyallen 9:7be40e42e578 213 ,imu.read_reg0(0x08),imu.read_reg0(0x09)
tommyallen 9:7be40e42e578 214 ,imu.read_reg0(0x0A),imu.read_reg0(0x0B)
tommyallen 9:7be40e42e578 215 ,imu.read_reg0(0x0C),imu.read_reg0(0x0D));
tommyallen 10:acbb851070c2 216 pc.printf("Time:,%d,/",timeNow);
tommyallen 10:acbb851070c2 217 timeStamp = timeNow;
tommyallen 9:7be40e42e578 218 }
tommyallen 8:41d1f9ab3be0 219 }
tommyallen 8:41d1f9ab3be0 220 while (RAW == false) {
tommyallen 9:7be40e42e578 221 Time = t.read_ms();
tommyallen 10:acbb851070c2 222 timeNow = + Time - timeOfStart;
tommyallen 10:acbb851070c2 223 if (timeNow > timeStamp + 9) {
tommyallen 9:7be40e42e578 224 imu.get_linear_accel(&linear_acc);
tommyallen 10:acbb851070c2 225 pc.printf("FUS,Time:,%d,x:,%+6.1f,y:,%+6.1f,z:,%+6.1f, , , , , , , , , /"
tommyallen 10:acbb851070c2 226 ,timeNow,linear_acc.x ,linear_acc.y ,linear_acc.z);
tommyallen 10:acbb851070c2 227 timeStamp = timeNow;
tommyallen 9:7be40e42e578 228 }
tommyallen 8:41d1f9ab3be0 229 }
tommyallen 8:41d1f9ab3be0 230 }
kenjiArai 4:6d1118089a36 231 }
kenjiArai 4:6d1118089a36 232
tommyallen 8:41d1f9ab3be0 233 // imu.get_Euler_Angles(&euler_angles);
tommyallen 8:41d1f9ab3be0 234 // pc.printf("X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
tommyallen 8:41d1f9ab3be0 235 // euler_angles.h, euler_angles.r, euler_angles.p);
tommyallen 8:41d1f9ab3be0 236 // imu.get_linear_accel(&linear_acc);
tommyallen 8:41d1f9ab3be0 237 // pc.printf("Acc:,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
tommyallen 8:41d1f9ab3be0 238 // linear_acc.x, linear_acc.y, linear_acc.z);
tommyallen 8:41d1f9ab3be0 239 //
tommyallen 8:41d1f9ab3be0 240 // imu.get_gravity(&gravity);
tommyallen 8:41d1f9ab3be0 241 // pc.printf("Gravity,X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
tommyallen 8:41d1f9ab3be0 242 // gravity.x, gravity.y, gravity.z);
tommyallen 8:41d1f9ab3be0 243 //
tommyallen 8:41d1f9ab3be0 244 // pc.printf("Time:,%d,\r\n",t.read_ms());
tommyallen 6:0590c7ff8c34 245 // imu.get_Euler_Angles(&euler_angles);
tommyallen 6:0590c7ff8c34 246 // pc.printf("[E],Y,%+6.1f,R,%+6.1f,P,%+6.1f,",
tommyallen 6:0590c7ff8c34 247 // euler_angles.h, euler_angles.r, euler_angles.p);
tommyallen 6:0590c7ff8c34 248 // imu.get_quaternion(&quaternion);
tommyallen 6:0590c7ff8c34 249 // pc.printf("[Q],W,%d,X,%d,Y,%d,Z,%d,",
tommyallen 6:0590c7ff8c34 250 // quaternion.w, quaternion.x, quaternion.y, quaternion.z);
tommyallen 6:0590c7ff8c34 251 // imu.get_linear_accel(&linear_acc);
tommyallen 6:0590c7ff8c34 252 // pc.printf("[L],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
tommyallen 6:0590c7ff8c34 253 // linear_acc.x, linear_acc.y, linear_acc.z);
tommyallen 6:0590c7ff8c34 254 // imu.get_gravity(&gravity);
tommyallen 6:0590c7ff8c34 255 // pc.printf("[G],X,%+6.1f,Y,%+6.1f,Z,%+6.1f,",
tommyallen 6:0590c7ff8c34 256 // gravity.x, gravity.y, gravity.z);
tommyallen 6:0590c7ff8c34 257 // imu.get_chip_temperature(&chip_temp);
tommyallen 6:0590c7ff8c34 258 // pc.printf("[T],%+d,%+d,",
tommyallen 6:0590c7ff8c34 259 // chip_temp.acc_chip, chip_temp.gyr_chip);
tommyallen 6:0590c7ff8c34 260 // pc.printf("[S],0x%x,[M],%d\r\n",
tommyallen 6:0590c7ff8c34 261 // imu.read_calib_status(), t.read_ms());
tommyallen 7:d4b5e83c7947 262 // pc.printf("Words\r\n");
kenjiArai 0:31451519d283 263