Sound update
Dependencies: 4DGL-uLCD-SE Physac-MBED PinDetect SDFileSystem mbed-rtos mbed
main.cpp@8:005b0a85be70, 20 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?
User | Revision | Line number | New 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 | } |