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

Dependencies:   BNO055 mbed

Files at this revision

API Documentation at this revision

Comitter:
jvfausto
Date:
Mon Jul 16 18:42:18 2018 +0000
Commit message:
All works;

Changed in this revision

BNO055.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r ebfdfa4739aa BNO055.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BNO055.lib	Mon Jul 16 18:42:18 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/StressedDave/code/BNO055/#1f722ffec323
diff -r 000000000000 -r ebfdfa4739aa main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jul 16 18:42:18 2018 +0000
@@ -0,0 +1,64 @@
+ #include    "mbed.h"
+ #include    "math.h"
+ #include    "BNO055.h"
+ 
+ #define PI 3.141593
+ 
+ Serial pc(USBTX,USBRX);
+ BNO055 imu(PTE25, PTE24);         // SDA, SCL 
+ 
+ double y;
+ double x;
+ double z;
+ double angleToNorth;
+ double result;
+ double pitchAcc;
+ double rollAcc;
+ double pitch = 0;
+ double roll = 0;
+ double yaw = 0;
+ Timer t;
+ 
+ int main() {
+     imu.reset();
+     pc.printf("Bosch Sensortec BNO055 test program on \r\n");//" __DATE__ "/" __TIME__ "\r\n");
+     while (imu.check() == 0){
+         pc.printf("Bosch BNO055 is NOT available!!\r\n");
+         wait(.5);
+     }
+    pc.printf("BNO055 found\r\n\r\n");
+    pc.printf("Chip          ID: %0z\r\n",imu.ID.id);
+    pc.printf("Accelerometer ID: %0z\r\n",imu.ID.accel);
+    pc.printf("Gyroscope     ID: %0z\r\n",imu.ID.gyro);
+    pc.printf("Magnetometer  ID: %0z\r\n\r\n",imu.ID.mag);
+    pc.printf("Firmware version v%d.%0d\r\n",imu.ID.sw[0],imu.ID.sw[1]);
+    pc.printf("Bootloader version v%d\r\n\r\n",imu.ID.bootload);
+    imu.set_accel_units(MPERSPERS);
+    imu.setmode(OPERATION_MODE_AMG);
+    imu.read_calibration_data();
+    imu.write_calibration_data();
+    imu.set_angle_units(DEGREES);
+    t.start();
+    while(1)
+    {
+        imu.setmode(OPERATION_MODE_AMG);
+        imu.get_accel();
+        imu.get_gyro();
+
+        pitch = atan2 (imu.accel.y ,( sqrt ((imu.accel.x * imu.accel.x) + (imu.accel.z * imu.accel.z))));
+        roll = atan2(-imu.accel.x ,( sqrt((imu.accel.y * imu.accel.y) + (imu.accel.z * imu.accel.z))));
+
+
+        roll = roll*57.3;
+        pitch = pitch*57.3;
+
+        yaw = (yaw - t.read()*imu.gyro.z);
+        t.reset();
+        if(yaw > 360)
+            yaw -= 360;
+        if(yaw < 0)
+            yaw += 360;
+
+        pc.printf("Euler angles: x %f\ty %f\t z %f \r\n\n", pitch, roll, yaw);
+    }
+}
\ No newline at end of file
diff -r 000000000000 -r ebfdfa4739aa mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Jul 16 18:42:18 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/a7c7b631e539
\ No newline at end of file