Jesus Fausto / Mbed 2 deprecated LMU

Get all but euler. Absolute North to

Dependencies:   BNO055 mbed

main.cpp@0:c9ec02922858, 2018-07-16 (annotated)

Committer:
jvfausto
Date:
Mon Jul 16 18:40:15 2018 +0000
Revision:
0:c9ec02922858
With absolute relativity to north

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jvfausto 0:c9ec02922858 1 #include "mbed.h"
jvfausto 0:c9ec02922858 2 #include "math.h"
jvfausto 0:c9ec02922858 3 #include "BNO055.h"
jvfausto 0:c9ec02922858 4
jvfausto 0:c9ec02922858 5 #define PI 3.141593
jvfausto 0:c9ec02922858 6
jvfausto 0:c9ec02922858 7 Serial pc(USBTX,USBRX);
jvfausto 0:c9ec02922858 8 BNO055 imu(PTE25, PTE24); // SDA, SCL
jvfausto 0:c9ec02922858 9
jvfausto 0:c9ec02922858 10 double y;
jvfausto 0:c9ec02922858 11 double x;
jvfausto 0:c9ec02922858 12 double z;
jvfausto 0:c9ec02922858 13 double angleToNorth;
jvfausto 0:c9ec02922858 14 double result;
jvfausto 0:c9ec02922858 15 // BNO055_ID_INF_TypeDef bno055_id_inf;
jvfausto 0:c9ec02922858 16 // BNO055_EULER_TypeDef euler_angles;
jvfausto 0:c9ec02922858 17
jvfausto 0:c9ec02922858 18 int main() {
jvfausto 0:c9ec02922858 19 // setup();
jvfausto 0:c9ec02922858 20 imu.reset();
jvfausto 0:c9ec02922858 21 pc.printf("Bosch Sensortec BNO055 test program on \r\n");//" __DATE__ "/" __TIME__ "\r\n");
jvfausto 0:c9ec02922858 22 while (imu.check() == 0){
jvfausto 0:c9ec02922858 23 pc.printf("Bosch BNO055 is NOT available!!\r\n");
jvfausto 0:c9ec02922858 24 wait(.5);
jvfausto 0:c9ec02922858 25 }
jvfausto 0:c9ec02922858 26 // pc.printf("Bosch Sensortec BNO055 available \r\n");//" __DATE__ "/" __TIME__ "\r\n");
jvfausto 0:c9ec02922858 27 pc.printf("BNO055 found\r\n\r\n");
jvfausto 0:c9ec02922858 28 pc.printf("Chip ID: %0z\r\n",imu.ID.id);
jvfausto 0:c9ec02922858 29 pc.printf("Accelerometer ID: %0z\r\n",imu.ID.accel);
jvfausto 0:c9ec02922858 30 pc.printf("Gyroscope ID: %0z\r\n",imu.ID.gyro);
jvfausto 0:c9ec02922858 31 pc.printf("Magnetometer ID: %0z\r\n\r\n",imu.ID.mag);
jvfausto 0:c9ec02922858 32 pc.printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]);
jvfausto 0:c9ec02922858 33 pc.printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload);
jvfausto 0:c9ec02922858 34 imu.set_accel_units(MPERSPERS);
jvfausto 0:c9ec02922858 35 imu.setmode(OPERATION_MODE_AMG);
jvfausto 0:c9ec02922858 36 imu.read_calibration_data();
jvfausto 0:c9ec02922858 37 imu.write_calibration_data();
jvfausto 0:c9ec02922858 38 imu.set_angle_units(DEGREES);
jvfausto 0:c9ec02922858 39 while(1)
jvfausto 0:c9ec02922858 40 {
jvfausto 0:c9ec02922858 41 imu.setmode(OPERATION_MODE_AMG);
jvfausto 0:c9ec02922858 42 imu.get_accel();
jvfausto 0:c9ec02922858 43 pc.printf("acceleration: x %f\ty %f\tz %f \r\n", imu.accel.x, imu.accel.y+.25, imu.accel.z);
jvfausto 0:c9ec02922858 44 imu.get_gyro();
jvfausto 0:c9ec02922858 45 pc.printf("gyroscope: x %f\ty %f\tz %f \r\n", imu.gyro.x, imu.gyro.y, imu.gyro.z);
jvfausto 0:c9ec02922858 46 imu.get_mag();
jvfausto 0:c9ec02922858 47 pc.printf("magnometer: x %f\ty %f\t %f \r\n", imu.mag.x, imu.mag.y, imu.mag.z);
jvfausto 0:c9ec02922858 48 x = imu.mag.x;
jvfausto 0:c9ec02922858 49 y = imu.mag.y;
jvfausto 0:c9ec02922858 50
jvfausto 0:c9ec02922858 51 result = x/y;
jvfausto 0:c9ec02922858 52
jvfausto 0:c9ec02922858 53 if(imu.mag.y>0)
jvfausto 0:c9ec02922858 54 angleToNorth = 90.0 - atan(result)*180/PI;
jvfausto 0:c9ec02922858 55 else if(imu.mag.y<0)
jvfausto 0:c9ec02922858 56 angleToNorth = 270.0 - atan(result)*180/PI;
jvfausto 0:c9ec02922858 57 else if(y == 0 && x <= 0)
jvfausto 0:c9ec02922858 58 angleToNorth = 180;
jvfausto 0:c9ec02922858 59 else if(y == 0 && x > 0)
jvfausto 0:c9ec02922858 60 angleToNorth = 0;
jvfausto 0:c9ec02922858 61 pc.printf("it is pointing %f angle from north \r\n", angleToNorth);
jvfausto 0:c9ec02922858 62 wait(1);
jvfausto 0:c9ec02922858 63 }
jvfausto 0:c9ec02922858 64 }