Jesus Fausto / Mbed 2 deprecated LMU_Euler

Euler values. Yaw is relative to initial position. Pitch and Roll are based on the ground.

Dependencies:   BNO055 mbed

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

Committer:
jvfausto
Date:
Mon Jul 16 18:42:18 2018 +0000
Revision:
0:ebfdfa4739aa
All works;

Who changed what in which revision?

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