Evan Brown / Mbed 2 deprecated APpart3_E_start_2

completed code

Dependencies:   mbed

main.cpp@5:4af75af374cc, 2018-11-22 (annotated)

Committer:
evenbrownie
Date:
Thu Nov 22 22:40:15 2018 +0000
Revision:
5:4af75af374cc
Parent:
4:4b6f0c4cd39f 
blah

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tlee6414 2:a4d5e7f96e87 1 #include "sensor_fusion.h"
evenbrownie 3:461a9012682d 2 #include "quaternion.h"
evenbrownie 3:461a9012682d 3 #include "millis.h"
evenbrownie 3:461a9012682d 4 #include "mbed.h"
evenbrownie 3:461a9012682d 5
evenbrownie 0:f43994f44684 6
tlee6414 2:a4d5e7f96e87 7 MPU6050 mpu(SDA,SCL);
evenbrownie 0:f43994f44684 8
evenbrownie 3:461a9012682d 9 Serial pc(USBTX,USBRX,115200);
evenbrownie 3:461a9012682d 10
evenbrownie 3:461a9012682d 11
evenbrownie 3:461a9012682d 12 void normalize(struct vector *raw, struct vector *normalized);
evenbrownie 3:461a9012682d 13
evenbrownie 0:f43994f44684 14 int main() {
tlee6414 2:a4d5e7f96e87 15 mpu.start();
evenbrownie 3:461a9012682d 16 millis_begin();
evenbrownie 3:461a9012682d 17 struct vector orientation = {0, 0, 1}; //starting point
tlee6414 4:4b6f0c4cd39f 18 struct vector gyro_previous = {0, 0, 0};
tlee6414 4:4b6f0c4cd39f 19 struct vector gyro = {0, 0, 0};
tlee6414 4:4b6f0c4cd39f 20 struct vector gyro_avg;
tlee6414 4:4b6f0c4cd39f 21 struct vector xUnit = {1, 0, 0};
tlee6414 4:4b6f0c4cd39f 22 struct vector yUnit = {0, 1, 0};
tlee6414 4:4b6f0c4cd39f 23 struct vector zUnit = {0, 0, 1};
evenbrownie 5:4af75af374cc 24 struct vector filteredGyro= {0,0,0};
evenbrownie 5:4af75af374cc 25 struct vector filteredAccel = {0,0,0};
tlee6414 4:4b6f0c4cd39f 26 struct quaternion xRot;
tlee6414 4:4b6f0c4cd39f 27 struct quaternion yRot;
tlee6414 4:4b6f0c4cd39f 28 struct quaternion zRot;
tlee6414 4:4b6f0c4cd39f 29 struct quaternion finalRot;
tlee6414 4:4b6f0c4cd39f 30 float time_current = 0;
tlee6414 4:4b6f0c4cd39f 31 float time_previous = 0;
tlee6414 4:4b6f0c4cd39f 32 float delta_time = 0;
tlee6414 4:4b6f0c4cd39f 33 float radFactor = (3.14159265358979323846/180)/15.3;
evenbrownie 5:4af75af374cc 34 float gyroFilterConstant = 0.0025;
evenbrownie 5:4af75af374cc 35 float accelFilterConstant = 15000;
evenbrownie 5:4af75af374cc 36 float accelNegativeFilterConstant = -15000;
tlee6414 4:4b6f0c4cd39f 37 struct vector gAngularV = {0, 0, 0};
evenbrownie 5:4af75af374cc 38 struct vector finalO;
evenbrownie 5:4af75af374cc 39 float alpha = 0.3;
tlee6414 4:4b6f0c4cd39f 40
tlee6414 2:a4d5e7f96e87 41
evenbrownie 0:f43994f44684 42 while(1) {
tlee6414 2:a4d5e7f96e87 43 if (!mpu.data_ready()) {
evenbrownie 5:4af75af374cc 44 //pc.printf("%b", mpu.data_ready());
tlee6414 2:a4d5e7f96e87 45 continue;
tlee6414 2:a4d5e7f96e87 46 }
tlee6414 2:a4d5e7f96e87 47
evenbrownie 3:461a9012682d 48 //raw data
evenbrownie 3:461a9012682d 49 struct vector accel;
tlee6414 4:4b6f0c4cd39f 50
tlee6414 4:4b6f0c4cd39f 51
evenbrownie 3:461a9012682d 52
evenbrownie 3:461a9012682d 53 //bias
evenbrownie 3:461a9012682d 54 struct vector gyro_bias = {0, 0, 0};
evenbrownie 5:4af75af374cc 55 struct vector accel_bias = {1300, -500, 16500};
evenbrownie 3:461a9012682d 56
evenbrownie 3:461a9012682d 57 //gets raw data
evenbrownie 3:461a9012682d 58 mpu.read_raw(&gyro.x, &gyro.y, &gyro.z, &accel.x, &accel.y, &accel.z);
evenbrownie 3:461a9012682d 59 //pc.printf("Accel: (%f, %f, %f)\n Gyro: (%f, %f, %f)\r\n", accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z);
evenbrownie 3:461a9012682d 60 //pc.printf( "%f %f %f\r\n", accel.x, accel.y, accel.z);
evenbrownie 3:461a9012682d 61 //pc.printf("%f %f %f\r\n", gyro.x, gyro.y, gyro.z);
evenbrownie 3:461a9012682d 62
tlee6414 4:4b6f0c4cd39f 63 //calculate angle
tlee6414 4:4b6f0c4cd39f 64 time_current = (float(millis()) / 1000);
tlee6414 4:4b6f0c4cd39f 65 delta_time = time_current - time_previous;
tlee6414 4:4b6f0c4cd39f 66 vector_add(&gyro, &gyro_previous, &gyro_avg);
evenbrownie 5:4af75af374cc 67 vector_multiply(&gyro_avg, -0.5, &gAngularV);
tlee6414 4:4b6f0c4cd39f 68 vector_multiply(&gAngularV, delta_time, &gAngularV);
tlee6414 4:4b6f0c4cd39f 69 gyro_previous = gyro;
tlee6414 4:4b6f0c4cd39f 70 time_previous = time_current;
tlee6414 4:4b6f0c4cd39f 71
tlee6414 4:4b6f0c4cd39f 72 //rotate orientation vector
tlee6414 4:4b6f0c4cd39f 73 gAngularV.x *= radFactor;
tlee6414 4:4b6f0c4cd39f 74 gAngularV.y *= radFactor;
evenbrownie 5:4af75af374cc 75 gAngularV.z *= radFactor;
evenbrownie 5:4af75af374cc 76
evenbrownie 5:4af75af374cc 77
evenbrownie 5:4af75af374cc 78 //pc.printf("%f %f %f\r\n", gAngularV.x,gAngularV.y,gAngularV.z);
evenbrownie 5:4af75af374cc 79 //wait(0.1);
tlee6414 4:4b6f0c4cd39f 80 quaternion_create(&xUnit, gAngularV.x, &xRot);
tlee6414 4:4b6f0c4cd39f 81 quaternion_create(&yUnit, gAngularV.y, &yRot);
tlee6414 4:4b6f0c4cd39f 82 quaternion_create(&zUnit, gAngularV.z, &zRot);
tlee6414 4:4b6f0c4cd39f 83 quaternion_multiply(&xRot, &yRot, &finalRot);
tlee6414 4:4b6f0c4cd39f 84 quaternion_multiply(&finalRot, &zRot, &finalRot);
tlee6414 4:4b6f0c4cd39f 85 quaternion_rotate(&orientation, &finalRot, &orientation);
tlee6414 2:a4d5e7f96e87 86
evenbrownie 5:4af75af374cc 87 //adds bias
evenbrownie 3:461a9012682d 88 vector_add(&accel, &accel_bias, &accel);
evenbrownie 3:461a9012682d 89 vector_add(&gyro, &gyro_bias, &gyro);
evenbrownie 5:4af75af374cc 90
evenbrownie 5:4af75af374cc 91 //create filtered vectors
evenbrownie 5:4af75af374cc 92 filteredGyro= orientation;
evenbrownie 5:4af75af374cc 93 filteredAccel = accel;
evenbrownie 5:4af75af374cc 94
evenbrownie 5:4af75af374cc 95 vector_normalize(&orientation, &orientation);
evenbrownie 5:4af75af374cc 96
evenbrownie 5:4af75af374cc 97
evenbrownie 3:461a9012682d 98 //pc.printf( "%f %f %f\r\n", accel.x, accel.y, accel.z);
tlee6414 4:4b6f0c4cd39f 99 //pc.printf("%f %f %f\r\n", gyro.x, gyro.y, gyro.z);
evenbrownie 5:4af75af374cc 100 //wait(0.1);
evenbrownie 5:4af75af374cc 101
evenbrownie 5:4af75af374cc 102
evenbrownie 5:4af75af374cc 103
evenbrownie 5:4af75af374cc 104 //Filter the change in angle so low values don't pass
evenbrownie 5:4af75af374cc 105 if(fabs(gAngularV.x) < gyroFilterConstant)
evenbrownie 5:4af75af374cc 106 {
evenbrownie 5:4af75af374cc 107 filteredGyro.x = 0;
evenbrownie 5:4af75af374cc 108 }
evenbrownie 5:4af75af374cc 109 if(fabs(gAngularV.y) < gyroFilterConstant)
evenbrownie 5:4af75af374cc 110 {
evenbrownie 5:4af75af374cc 111 filteredGyro.y = 0;
evenbrownie 5:4af75af374cc 112 }
evenbrownie 5:4af75af374cc 113 if(fabs(gAngularV.z) < gyroFilterConstant)
evenbrownie 5:4af75af374cc 114 {
evenbrownie 5:4af75af374cc 115 filteredGyro.z = 0;
evenbrownie 5:4af75af374cc 116 }
tlee6414 4:4b6f0c4cd39f 117
evenbrownie 5:4af75af374cc 118 //Filter the accel data so high values don't pass
evenbrownie 5:4af75af374cc 119 if(accel.x > accelFilterConstant)
evenbrownie 5:4af75af374cc 120 {
evenbrownie 5:4af75af374cc 121 filteredAccel.x = accelFilterConstant;
evenbrownie 5:4af75af374cc 122 }
evenbrownie 5:4af75af374cc 123 if(accel.y > accelFilterConstant)
evenbrownie 5:4af75af374cc 124 {
evenbrownie 5:4af75af374cc 125 filteredAccel.y = accelFilterConstant;
evenbrownie 5:4af75af374cc 126 }
evenbrownie 5:4af75af374cc 127 if(accel.z > accelFilterConstant)
evenbrownie 5:4af75af374cc 128 {
evenbrownie 5:4af75af374cc 129 filteredAccel.z = accelFilterConstant;
evenbrownie 5:4af75af374cc 130 }
evenbrownie 5:4af75af374cc 131
evenbrownie 5:4af75af374cc 132 //filter the negative high values
evenbrownie 5:4af75af374cc 133 if(accel.x < accelNegativeFilterConstant)
evenbrownie 5:4af75af374cc 134 {
evenbrownie 5:4af75af374cc 135 filteredAccel.x = accelNegativeFilterConstant;
evenbrownie 5:4af75af374cc 136 }
evenbrownie 5:4af75af374cc 137 if(accel.y < accelNegativeFilterConstant)
evenbrownie 5:4af75af374cc 138 {
evenbrownie 5:4af75af374cc 139 filteredAccel.y = accelNegativeFilterConstant;
evenbrownie 5:4af75af374cc 140 }
evenbrownie 5:4af75af374cc 141 if(accel.z < accelNegativeFilterConstant)
evenbrownie 5:4af75af374cc 142 {
evenbrownie 5:4af75af374cc 143 filteredAccel.z = accelNegativeFilterConstant;
evenbrownie 5:4af75af374cc 144 }
evenbrownie 5:4af75af374cc 145
evenbrownie 5:4af75af374cc 146 //calculate filtered orientation
evenbrownie 5:4af75af374cc 147 vector_multiply(&filteredAccel, alpha, &filteredAccel);
evenbrownie 5:4af75af374cc 148 vector_multiply(&filteredGyro, (1 - alpha), &filteredGyro);
evenbrownie 5:4af75af374cc 149 vector_add(&filteredAccel, &filteredGyro, &finalO);
evenbrownie 5:4af75af374cc 150 vector_normalize(&finalO, &finalO);
evenbrownie 5:4af75af374cc 151
evenbrownie 3:461a9012682d 152 //normalize raw data to get vector
evenbrownie 3:461a9012682d 153 struct vector accel_norm = {0, 0, 0};
evenbrownie 3:461a9012682d 154 vector_normalize(&accel, &accel_norm);
evenbrownie 5:4af75af374cc 155 pc.printf("%f %f %f %f %f %f %f %f %f\r\n", accel_norm.x,accel_norm.y,accel_norm.z, orientation.x, orientation.y, orientation.z, finalO.x, finalO.y, finalO.z);
evenbrownie 3:461a9012682d 156
evenbrownie 5:4af75af374cc 157 wait(0.1);
evenbrownie 0:f43994f44684 158 }
evenbrownie 3:461a9012682d 159 }