Jay Balar / Mbed 2 deprecated 4180_Project_3

Sound update

Dependencies:   4DGL-uLCD-SE Physac-MBED PinDetect SDFileSystem mbed-rtos mbed

main.cpp@8:005b0a85be70, 19 months ago (annotated)

Committer:
jstephens78
Date:
Tue Nov 15 05:42:41 2022 +0000
Revision:
8:005b0a85be70
Parent:
7:d27f97ac2bea 
Playing with Fusion AHRS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jstephens78 8:005b0a85be70 1 /**
jstephens78 8:005b0a85be70 2 * Uses the Fusion library for AHRS algorithm:
jstephens78 8:005b0a85be70 3 * https://github.com/xioTechnologies/Fusion
jstephens78 8:005b0a85be70 4 */
jstephens78 8:005b0a85be70 5
jstephens78 8:005b0a85be70 6
jstephens78 7:d27f97ac2bea 7 #include <cmath>
jstephens78 0:da114b98e013 8 #include "mbed.h"
jstephens78 7:d27f97ac2bea 9 #include "uLCD_4DGL.h"
jstephens78 7:d27f97ac2bea 10 #include "LSM9DS1.h"
jstephens78 7:d27f97ac2bea 11 #define PI 3.14159
jstephens78 8:005b0a85be70 12 #include "Fusion/Fusion.h"
jstephens78 8:005b0a85be70 13 #define SAMPLE_RATE (100) // replace this with actual sample rate
jstephens78 7:d27f97ac2bea 14
jstephens78 7:d27f97ac2bea 15 uLCD_4DGL uLCD(p9,p10,p30);
jstephens78 7:d27f97ac2bea 16 LSM9DS1 imu(p28, p27, 0xD6, 0x3C);
jstephens78 7:d27f97ac2bea 17 Serial pc(USBTX, USBRX);
jstephens78 0:da114b98e013 18
jstephens78 7:d27f97ac2bea 19 void getAttitude(LSM9DS1& imu, float& roll, float& pitch, float& yaw)
jstephens78 7:d27f97ac2bea 20 {
jstephens78 7:d27f97ac2bea 21 float ax = imu.calcAccel(imu.ax),
jstephens78 7:d27f97ac2bea 22 ay = imu.calcAccel(imu.ay),
jstephens78 7:d27f97ac2bea 23 az = imu.calcAccel(imu.az),
jstephens78 7:d27f97ac2bea 24 mx = -imu.calcMag(imu.mx),
jstephens78 7:d27f97ac2bea 25 my = imu.calcMag(imu.my),
jstephens78 7:d27f97ac2bea 26 mz = imu.calcMag(imu.mz);
jstephens78 7:d27f97ac2bea 27
jstephens78 7:d27f97ac2bea 28 pitch = atan2(-ax, sqrt(ay * ay + az * az));
jstephens78 7:d27f97ac2bea 29
jstephens78 7:d27f97ac2bea 30 if (az == 0.0) roll = (ay < 0.0) ? 180 : 0.0;
jstephens78 7:d27f97ac2bea 31 else roll = atan2(ay, az);
jstephens78 8:005b0a85be70 32
jstephens78 7:d27f97ac2bea 33 if (my == 0.0) yaw = (mx < 0.0) ? 180.0 : 0.0;
jstephens78 7:d27f97ac2bea 34 else yaw = atan2(mx, my);
jstephens78 8:005b0a85be70 35
jstephens78 7:d27f97ac2bea 36 if (yaw > 180.0) yaw -=PI;
jstephens78 7:d27f97ac2bea 37 else if (yaw < -180.0) yaw += PI;
jstephens78 7:d27f97ac2bea 38
jstephens78 7:d27f97ac2bea 39 }
jstephens78 0:da114b98e013 40
jstephens78 7:d27f97ac2bea 41 int main()
jstephens78 7:d27f97ac2bea 42 {
jstephens78 8:005b0a85be70 43 ////////////////////
jstephens78 8:005b0a85be70 44 // INITIALIZATION //
jstephens78 8:005b0a85be70 45 ////////////////////
jstephens78 8:005b0a85be70 46
jstephens78 7:d27f97ac2bea 47 imu.begin();
jstephens78 7:d27f97ac2bea 48 if (!imu.begin()) {
jstephens78 7:d27f97ac2bea 49 pc.printf("Failed to communicate with LSM9DS1.\n");
jstephens78 7:d27f97ac2bea 50 }
jstephens78 8:005b0a85be70 51 imu.setGyroScale(2000);
jstephens78 8:005b0a85be70 52 imu.setAccelScale(16);
jstephens78 7:d27f97ac2bea 53 imu.calibrate(1);
jstephens78 7:d27f97ac2bea 54
jstephens78 8:005b0a85be70 55 // Initialise algorithms
jstephens78 8:005b0a85be70 56 FusionAhrs ahrs;
jstephens78 8:005b0a85be70 57 FusionAhrsInitialise(&ahrs);
jstephens78 8:005b0a85be70 58
jstephens78 8:005b0a85be70 59 // Set AHRS algorithm settings
jstephens78 8:005b0a85be70 60 const FusionAhrsSettings settings = {
jstephens78 8:005b0a85be70 61 .gain = 0.5f,
jstephens78 8:005b0a85be70 62 .accelerationRejection = 10.0f,
jstephens78 8:005b0a85be70 63 .magneticRejection = 20.0f,
jstephens78 8:005b0a85be70 64 .rejectionTimeout = 5 * SAMPLE_RATE, /* 5 seconds */
jstephens78 8:005b0a85be70 65 };
jstephens78 8:005b0a85be70 66 FusionAhrsSetSettings(&ahrs, &settings);
jstephens78 8:005b0a85be70 67
jstephens78 8:005b0a85be70 68 Timer timer;
jstephens78 8:005b0a85be70 69 timer.start();
jstephens78 8:005b0a85be70 70
jstephens78 8:005b0a85be70 71
jstephens78 8:005b0a85be70 72
jstephens78 8:005b0a85be70 73 //////////////
jstephens78 8:005b0a85be70 74 // IMU LOOP //
jstephens78 8:005b0a85be70 75 //////////////
jstephens78 7:d27f97ac2bea 76
jstephens78 7:d27f97ac2bea 77 while (true) {
jstephens78 7:d27f97ac2bea 78 // Read IMU
jstephens78 7:d27f97ac2bea 79 while(!imu.accelAvailable());
jstephens78 7:d27f97ac2bea 80 imu.readAccel();
jstephens78 7:d27f97ac2bea 81
jstephens78 8:005b0a85be70 82 while (!imu.gyroAvailable());
jstephens78 8:005b0a85be70 83 imu.readGyro();
jstephens78 8:005b0a85be70 84
jstephens78 7:d27f97ac2bea 85 while (!imu.magAvailable());
jstephens78 7:d27f97ac2bea 86 imu.readMag();
jstephens78 7:d27f97ac2bea 87
jstephens78 8:005b0a85be70 88 // Elementary Algorithm:
jstephens78 8:005b0a85be70 89 //float roll, pitch, yaw;
jstephens78 8:005b0a85be70 90 //getAttitude(imu, roll, pitch, yaw);
jstephens78 8:005b0a85be70 91 //pc.printf("%7f %7f %7f\r\n", roll, pitch, yaw);
jstephens78 7:d27f97ac2bea 92
jstephens78 8:005b0a85be70 93 // Construct algorithm inputs
jstephens78 8:005b0a85be70 94 FusionVector gyroscope = { // Gyro data in degrees / s
jstephens78 8:005b0a85be70 95 imu.calcGyro(imu.gx),
jstephens78 8:005b0a85be70 96 imu.calcGyro(imu.gy),
jstephens78 8:005b0a85be70 97 imu.calcGyro(imu.gz)
jstephens78 8:005b0a85be70 98 };
jstephens78 8:005b0a85be70 99 FusionVector accelerometer = { // Accelerometer data in g
jstephens78 8:005b0a85be70 100 imu.calcAccel(imu.ax),
jstephens78 8:005b0a85be70 101 imu.calcAccel(imu.ay),
jstephens78 8:005b0a85be70 102 imu.calcAccel(imu.az)
jstephens78 8:005b0a85be70 103 };
jstephens78 8:005b0a85be70 104 FusionVector magnetometer = { // Accelerometer data in g
jstephens78 8:005b0a85be70 105 imu.calcMag(imu.mx),
jstephens78 8:005b0a85be70 106 imu.calcMag(imu.my),
jstephens78 8:005b0a85be70 107 imu.calcMag(imu.mz)
jstephens78 8:005b0a85be70 108 };
jstephens78 7:d27f97ac2bea 109
jstephens78 8:005b0a85be70 110 // Update AHRS algorithm
jstephens78 8:005b0a85be70 111 float dt = timer.read();
jstephens78 8:005b0a85be70 112 timer.reset();
jstephens78 8:005b0a85be70 113 FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, magnetometer, dt);
jstephens78 7:d27f97ac2bea 114
jstephens78 8:005b0a85be70 115 // Print algorithm outputs
jstephens78 8:005b0a85be70 116 const FusionEuler euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));
jstephens78 8:005b0a85be70 117 const FusionVector earth = FusionAhrsGetEarthAcceleration(&ahrs);
jstephens78 8:005b0a85be70 118
jstephens78 8:005b0a85be70 119 printf("R %5.1f P %5.1f Y %5.1f\n\r",
jstephens78 8:005b0a85be70 120 euler.angle.roll, euler.angle.pitch, euler.angle.yaw);
jstephens78 8:005b0a85be70 121
jstephens78 8:005b0a85be70 122 float roll = euler.angle.roll / 360 * 2 * PI,
jstephens78 8:005b0a85be70 123 pitch = euler.angle.pitch / 360 * 2 * PI,
jstephens78 8:005b0a85be70 124 yaw = euler.angle.yaw / 360 * 2 * PI;
jstephens78 7:d27f97ac2bea 125
jstephens78 7:d27f97ac2bea 126 uLCD.cls();
jstephens78 7:d27f97ac2bea 127
jstephens78 7:d27f97ac2bea 128 float x = 64 + roll * 60;
jstephens78 7:d27f97ac2bea 129 float y = 64 + pitch * 60;
jstephens78 8:005b0a85be70 130 float dx = 12 * cos(yaw);
jstephens78 8:005b0a85be70 131 float dy = 12 * sin(yaw);
jstephens78 7:d27f97ac2bea 132
jstephens78 7:d27f97ac2bea 133 uLCD.circle(x, y, 2, GREEN);
jstephens78 7:d27f97ac2bea 134 uLCD.line(x-dx, y-dy, x+dx, y+dy, GREEN);
jstephens78 7:d27f97ac2bea 135 wait(.05);
jstephens78 0:da114b98e013 136 }
jstephens78 0:da114b98e013 137 }