Jesus Fausto
/
LMU_Euler
Euler values. Yaw is relative to initial position. Pitch and Roll are based on the ground.
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?
User | Revision | Line number | New 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 | } |