Johan Beverini
/
Project_5A_2
For a school project
Fork of MPU6050IMU by
main.cpp@3:4c1180a712e3, 2018-03-15 (annotated)
- Committer:
- JohanBeverini
- Date:
- Thu Mar 15 15:02:02 2018 +0000
- Revision:
- 3:4c1180a712e3
- Parent:
- 1:cea9d83b8636
+ filtre + matrice euler
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
onehorse | 1:cea9d83b8636 | 1 | #include "mbed.h" |
onehorse | 1:cea9d83b8636 | 2 | #include "MPU6050.h" |
JohanBeverini | 3:4c1180a712e3 | 3 | #include <math.h> |
onehorse | 0:65aa78c10981 | 4 | |
onehorse | 0:65aa78c10981 | 5 | |
onehorse | 1:cea9d83b8636 | 6 | float sum = 0; |
onehorse | 1:cea9d83b8636 | 7 | uint32_t sumCount = 0; |
onehorse | 1:cea9d83b8636 | 8 | |
onehorse | 1:cea9d83b8636 | 9 | MPU6050 mpu6050; |
onehorse | 1:cea9d83b8636 | 10 | |
JohanBeverini | 3:4c1180a712e3 | 11 | AnalogOut ANA1(A3); |
JohanBeverini | 3:4c1180a712e3 | 12 | //AnalogOut ANA2(PA_5); |
JohanBeverini | 3:4c1180a712e3 | 13 | |
JohanBeverini | 3:4c1180a712e3 | 14 | Ticker ms; |
JohanBeverini | 3:4c1180a712e3 | 15 | |
onehorse | 1:cea9d83b8636 | 16 | Timer t; |
onehorse | 1:cea9d83b8636 | 17 | |
JohanBeverini | 3:4c1180a712e3 | 18 | Serial pc(SERIAL_TX, SERIAL_RX); // tx, rx |
JohanBeverini | 3:4c1180a712e3 | 19 | |
JohanBeverini | 3:4c1180a712e3 | 20 | Serial BT(PA_9, PA_10); // tx, rx |
JohanBeverini | 3:4c1180a712e3 | 21 | |
onehorse | 1:cea9d83b8636 | 22 | |
JohanBeverini | 3:4c1180a712e3 | 23 | |
JohanBeverini | 3:4c1180a712e3 | 24 | float alpha, betaa, gammaa; |
JohanBeverini | 3:4c1180a712e3 | 25 | float axx, ayy, azz; |
JohanBeverini | 3:4c1180a712e3 | 26 | float poid[3]; |
JohanBeverini | 3:4c1180a712e3 | 27 | float a, b, c, d, e, s; |
JohanBeverini | 3:4c1180a712e3 | 28 | int i; |
JohanBeverini | 3:4c1180a712e3 | 29 | float matrice[3][3], resultat[3]; |
JohanBeverini | 3:4c1180a712e3 | 30 | |
JohanBeverini | 3:4c1180a712e3 | 31 | bool first = true; |
JohanBeverini | 3:4c1180a712e3 | 32 | |
JohanBeverini | 3:4c1180a712e3 | 33 | bool tick_mili; |
JohanBeverini | 3:4c1180a712e3 | 34 | |
JohanBeverini | 3:4c1180a712e3 | 35 | float x_x_filter[3]={0,0,0}, x_y_filter[3]={0,0,0}; |
JohanBeverini | 3:4c1180a712e3 | 36 | float y_x_filter[3]={0,0,0}, y_y_filter[3]={0,0,0}; |
JohanBeverini | 3:4c1180a712e3 | 37 | float z_x_filter[3]={0,0,0}, z_y_filter[3]={0,0,0}; |
JohanBeverini | 3:4c1180a712e3 | 38 | float a_coef[3]={1.0000, -1.5610, 0.6414}; |
JohanBeverini | 3:4c1180a712e3 | 39 | float b_coef[3]={0.0201, 0.0402, 0.0201}; |
JohanBeverini | 3:4c1180a712e3 | 40 | float x_x_filter_ph[3]={0,0,0}, x_y_filter_ph[3]={0,0,0}; |
JohanBeverini | 3:4c1180a712e3 | 41 | float y_x_filter_ph[3]={0,0,0}, y_y_filter_ph[3]={0,0,0}; |
JohanBeverini | 3:4c1180a712e3 | 42 | float z_x_filter_ph[3]={0,0,0}, z_y_filter_ph[3]={0,0,0}; |
JohanBeverini | 3:4c1180a712e3 | 43 | float a_coef_ph[3]={1.0000, -1.9956, 0.9956}; |
JohanBeverini | 3:4c1180a712e3 | 44 | float b_coef_ph[3]={0.9978, -1.9956, 0.9978}; |
JohanBeverini | 3:4c1180a712e3 | 45 | float gx_filtre, gy_filtre, gz_filtre; |
JohanBeverini | 3:4c1180a712e3 | 46 | float gx_filtre2=0.0f, gy_filtre2=0.0f, gz_filtre2=0.0f; |
JohanBeverini | 3:4c1180a712e3 | 47 | float trapeze_x = 0.0f; |
JohanBeverini | 3:4c1180a712e3 | 48 | float trapeze_y = 0.0f; |
JohanBeverini | 3:4c1180a712e3 | 49 | float trapeze_z = 0.0f; |
JohanBeverini | 3:4c1180a712e3 | 50 | |
JohanBeverini | 3:4c1180a712e3 | 51 | void mili(void){ |
JohanBeverini | 3:4c1180a712e3 | 52 | tick_mili=true; |
JohanBeverini | 3:4c1180a712e3 | 53 | } |
JohanBeverini | 3:4c1180a712e3 | 54 | |
JohanBeverini | 3:4c1180a712e3 | 55 | |
onehorse | 1:cea9d83b8636 | 56 | |
onehorse | 1:cea9d83b8636 | 57 | int main() |
onehorse | 1:cea9d83b8636 | 58 | { |
onehorse | 1:cea9d83b8636 | 59 | pc.baud(9600); |
JohanBeverini | 3:4c1180a712e3 | 60 | BT.baud(9600); |
JohanBeverini | 3:4c1180a712e3 | 61 | |
JohanBeverini | 3:4c1180a712e3 | 62 | pc.printf("hello word\n"); |
JohanBeverini | 3:4c1180a712e3 | 63 | BT.printf("connection...\n"); |
onehorse | 0:65aa78c10981 | 64 | |
onehorse | 1:cea9d83b8636 | 65 | //Set up I2C |
onehorse | 1:cea9d83b8636 | 66 | i2c.frequency(400000); // use fast (400 kHz) I2C |
onehorse | 1:cea9d83b8636 | 67 | |
JohanBeverini | 3:4c1180a712e3 | 68 | alpha=0; |
JohanBeverini | 3:4c1180a712e3 | 69 | betaa=0; |
JohanBeverini | 3:4c1180a712e3 | 70 | gammaa=0; |
JohanBeverini | 3:4c1180a712e3 | 71 | |
JohanBeverini | 3:4c1180a712e3 | 72 | ms.attach(&mili, 0.001); |
onehorse | 1:cea9d83b8636 | 73 | t.start(); |
onehorse | 1:cea9d83b8636 | 74 | |
JohanBeverini | 3:4c1180a712e3 | 75 | //lcd.init(); |
JohanBeverini | 3:4c1180a712e3 | 76 | //lcd.setBrightness(0.05); |
onehorse | 1:cea9d83b8636 | 77 | |
onehorse | 1:cea9d83b8636 | 78 | |
onehorse | 1:cea9d83b8636 | 79 | // Read the WHO_AM_I register, this is a good test of communication |
onehorse | 1:cea9d83b8636 | 80 | uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 |
onehorse | 1:cea9d83b8636 | 81 | pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r"); |
onehorse | 1:cea9d83b8636 | 82 | |
onehorse | 1:cea9d83b8636 | 83 | if (whoami == 0x68) // WHO_AM_I should always be 0x68 |
onehorse | 1:cea9d83b8636 | 84 | { |
onehorse | 1:cea9d83b8636 | 85 | pc.printf("MPU6050 is online..."); |
onehorse | 1:cea9d83b8636 | 86 | wait(1); |
JohanBeverini | 3:4c1180a712e3 | 87 | //lcd.clear(); |
JohanBeverini | 3:4c1180a712e3 | 88 | //lcd.printString("MPU6050 OK", 0, 0); |
onehorse | 0:65aa78c10981 | 89 | |
onehorse | 1:cea9d83b8636 | 90 | |
onehorse | 1:cea9d83b8636 | 91 | mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values |
onehorse | 1:cea9d83b8636 | 92 | pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r"); |
onehorse | 1:cea9d83b8636 | 93 | pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r"); |
onehorse | 1:cea9d83b8636 | 94 | pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r"); |
onehorse | 1:cea9d83b8636 | 95 | pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r"); |
onehorse | 1:cea9d83b8636 | 96 | pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r"); |
onehorse | 1:cea9d83b8636 | 97 | pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r"); |
onehorse | 1:cea9d83b8636 | 98 | wait(1); |
onehorse | 0:65aa78c10981 | 99 | |
onehorse | 1:cea9d83b8636 | 100 | if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) |
onehorse | 1:cea9d83b8636 | 101 | { |
onehorse | 1:cea9d83b8636 | 102 | mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration |
onehorse | 1:cea9d83b8636 | 103 | mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers |
onehorse | 1:cea9d83b8636 | 104 | mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature |
onehorse | 0:65aa78c10981 | 105 | |
JohanBeverini | 3:4c1180a712e3 | 106 | //lcd.clear(); |
JohanBeverini | 3:4c1180a712e3 | 107 | //lcd.printString("MPU6050", 0, 0); |
JohanBeverini | 3:4c1180a712e3 | 108 | //lcd.printString("pass self test", 0, 1); |
JohanBeverini | 3:4c1180a712e3 | 109 | //lcd.printString("initializing", 0, 2); |
onehorse | 1:cea9d83b8636 | 110 | wait(2); |
onehorse | 1:cea9d83b8636 | 111 | } |
onehorse | 1:cea9d83b8636 | 112 | else |
onehorse | 1:cea9d83b8636 | 113 | { |
onehorse | 1:cea9d83b8636 | 114 | pc.printf("Device did not the pass self-test!\n\r"); |
onehorse | 0:65aa78c10981 | 115 | |
JohanBeverini | 3:4c1180a712e3 | 116 | //lcd.clear(); |
JohanBeverini | 3:4c1180a712e3 | 117 | //lcd.printString("MPU6050", 0, 0); |
JohanBeverini | 3:4c1180a712e3 | 118 | //lcd.printString("no pass", 0, 1); |
JohanBeverini | 3:4c1180a712e3 | 119 | //lcd.printString("self test", 0, 2); |
onehorse | 1:cea9d83b8636 | 120 | } |
onehorse | 1:cea9d83b8636 | 121 | } |
onehorse | 1:cea9d83b8636 | 122 | else |
onehorse | 1:cea9d83b8636 | 123 | { |
onehorse | 1:cea9d83b8636 | 124 | pc.printf("Could not connect to MPU6050: \n\r"); |
onehorse | 1:cea9d83b8636 | 125 | pc.printf("%#x \n", whoami); |
onehorse | 1:cea9d83b8636 | 126 | |
JohanBeverini | 3:4c1180a712e3 | 127 | //lcd.clear(); |
JohanBeverini | 3:4c1180a712e3 | 128 | //lcd.printString("MPU6050", 0, 0); |
JohanBeverini | 3:4c1180a712e3 | 129 | //lcd.printString("no connection", 0, 1); |
JohanBeverini | 3:4c1180a712e3 | 130 | //lcd.printString("0x", 0, 2); lcd.setXYAddress(20, 2); lcd.printChar(whoami); |
onehorse | 1:cea9d83b8636 | 131 | |
onehorse | 1:cea9d83b8636 | 132 | while(1) ; // Loop forever if communication doesn't happen |
onehorse | 0:65aa78c10981 | 133 | } |
onehorse | 0:65aa78c10981 | 134 | |
onehorse | 0:65aa78c10981 | 135 | |
onehorse | 0:65aa78c10981 | 136 | |
onehorse | 1:cea9d83b8636 | 137 | while(1) { |
onehorse | 0:65aa78c10981 | 138 | |
JohanBeverini | 3:4c1180a712e3 | 139 | if (tick_mili==true){ |
JohanBeverini | 3:4c1180a712e3 | 140 | tick_mili=false; |
JohanBeverini | 3:4c1180a712e3 | 141 | |
JohanBeverini | 3:4c1180a712e3 | 142 | |
onehorse | 1:cea9d83b8636 | 143 | // If data ready bit set, all data registers have new data |
onehorse | 1:cea9d83b8636 | 144 | if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt |
onehorse | 1:cea9d83b8636 | 145 | mpu6050.readAccelData(accelCount); // Read the x/y/z adc values |
onehorse | 1:cea9d83b8636 | 146 | mpu6050.getAres(); |
onehorse | 0:65aa78c10981 | 147 | |
onehorse | 0:65aa78c10981 | 148 | // Now we'll calculate the accleration value into actual g's |
onehorse | 0:65aa78c10981 | 149 | ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set |
onehorse | 0:65aa78c10981 | 150 | ay = (float)accelCount[1]*aRes - accelBias[1]; |
onehorse | 0:65aa78c10981 | 151 | az = (float)accelCount[2]*aRes - accelBias[2]; |
onehorse | 0:65aa78c10981 | 152 | |
onehorse | 1:cea9d83b8636 | 153 | mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values |
onehorse | 1:cea9d83b8636 | 154 | mpu6050.getGres(); |
onehorse | 0:65aa78c10981 | 155 | |
onehorse | 0:65aa78c10981 | 156 | // Calculate the gyro value into actual degrees per second |
onehorse | 1:cea9d83b8636 | 157 | gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set |
onehorse | 1:cea9d83b8636 | 158 | gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; |
JohanBeverini | 3:4c1180a712e3 | 159 | gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; |
JohanBeverini | 3:4c1180a712e3 | 160 | |
onehorse | 0:65aa78c10981 | 161 | |
onehorse | 1:cea9d83b8636 | 162 | tempCount = mpu6050.readTempData(); // Read the x/y/z adc values |
onehorse | 0:65aa78c10981 | 163 | temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade |
onehorse | 0:65aa78c10981 | 164 | } |
onehorse | 0:65aa78c10981 | 165 | |
onehorse | 0:65aa78c10981 | 166 | Now = t.read_us(); |
onehorse | 1:cea9d83b8636 | 167 | deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update |
onehorse | 0:65aa78c10981 | 168 | lastUpdate = Now; |
onehorse | 1:cea9d83b8636 | 169 | |
onehorse | 1:cea9d83b8636 | 170 | sum += deltat; |
onehorse | 1:cea9d83b8636 | 171 | sumCount++; |
onehorse | 1:cea9d83b8636 | 172 | |
onehorse | 0:65aa78c10981 | 173 | if(lastUpdate - firstUpdate > 10000000.0f) { |
onehorse | 1:cea9d83b8636 | 174 | beta = 0.04; // decrease filter gain after stabilized |
onehorse | 1:cea9d83b8636 | 175 | zeta = 0.015; // increasey bias drift gain after stabilized |
onehorse | 0:65aa78c10981 | 176 | } |
onehorse | 1:cea9d83b8636 | 177 | |
onehorse | 0:65aa78c10981 | 178 | // Pass gyro rate as rad/s |
JohanBeverini | 3:4c1180a712e3 | 179 | //mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); |
JohanBeverini | 3:4c1180a712e3 | 180 | |
JohanBeverini | 3:4c1180a712e3 | 181 | |
JohanBeverini | 3:4c1180a712e3 | 182 | //gx*=PI/180.0f; |
JohanBeverini | 3:4c1180a712e3 | 183 | //gy*=PI/180.0f; |
JohanBeverini | 3:4c1180a712e3 | 184 | //gz*=PI/180.0f; |
JohanBeverini | 3:4c1180a712e3 | 185 | //gx/=1000.0f; |
JohanBeverini | 3:4c1180a712e3 | 186 | //gy/=1000.0f; |
JohanBeverini | 3:4c1180a712e3 | 187 | //gz/=1000.0f; |
JohanBeverini | 3:4c1180a712e3 | 188 | |
JohanBeverini | 3:4c1180a712e3 | 189 | ////////filtre PB 100Hz / PH 1Hz |
JohanBeverini | 3:4c1180a712e3 | 190 | //x_x_filter[6]=x_x_filter[5]; x_x_filter[5]=x_x_filter[4]; x_x_filter[4]=x_x_filter[3]; |
JohanBeverini | 3:4c1180a712e3 | 191 | //x_x_filter[3]=x_x_filter[2]; |
JohanBeverini | 3:4c1180a712e3 | 192 | x_x_filter[2]=x_x_filter[1]; x_x_filter[1]=x_x_filter[0]; |
JohanBeverini | 3:4c1180a712e3 | 193 | x_x_filter[0]=gx; |
JohanBeverini | 3:4c1180a712e3 | 194 | //x_y_filter[6]=x_y_filter[5]; x_y_filter[5]=x_y_filter[4]; x_y_filter[4]=x_y_filter[3]; |
JohanBeverini | 3:4c1180a712e3 | 195 | //x_y_filter[3]=x_y_filter[2]; |
JohanBeverini | 3:4c1180a712e3 | 196 | x_y_filter[2]=x_y_filter[1]; x_y_filter[1]=x_y_filter[0]; |
JohanBeverini | 3:4c1180a712e3 | 197 | x_y_filter[0]=b_coef[0]*x_x_filter[0]+b_coef[1]*x_x_filter[1]+b_coef[2]*x_x_filter[2] //+b_coef[3]*x_x_filter[3] //+b_coef[4]*x_x_filter[4]+b_coef[5]*x_x_filter[5]+b_coef[6]*x_x_filter[6] |
JohanBeverini | 3:4c1180a712e3 | 198 | -(a_coef[1]*x_y_filter[1]+a_coef[2]*x_y_filter[2]); //+a_coef[3]*x_y_filter[3]); //+a_coef[4]*x_y_filter[4]+a_coef[5]*x_y_filter[5]+a_coef[6]*x_y_filter[6]); |
JohanBeverini | 3:4c1180a712e3 | 199 | gx_filtre=x_y_filter[0]; |
JohanBeverini | 3:4c1180a712e3 | 200 | |
JohanBeverini | 3:4c1180a712e3 | 201 | //y_x_filter[6]=y_x_filter[5]; y_x_filter[5]=y_x_filter[4]; y_x_filter[4]=y_x_filter[3]; |
JohanBeverini | 3:4c1180a712e3 | 202 | //y_x_filter[3]=y_x_filter[2]; |
JohanBeverini | 3:4c1180a712e3 | 203 | y_x_filter[2]=y_x_filter[1]; y_x_filter[1]=y_x_filter[0]; |
JohanBeverini | 3:4c1180a712e3 | 204 | y_x_filter[0]=gy; |
JohanBeverini | 3:4c1180a712e3 | 205 | //y_y_filter[6]=y_y_filter[5]; y_y_filter[5]=y_y_filter[4]; y_y_filter[4]=y_y_filter[3]; |
JohanBeverini | 3:4c1180a712e3 | 206 | //y_y_filter[3]=y_y_filter[2]; |
JohanBeverini | 3:4c1180a712e3 | 207 | y_y_filter[2]=y_y_filter[1]; y_y_filter[1]=y_y_filter[0]; |
JohanBeverini | 3:4c1180a712e3 | 208 | y_y_filter[0]=b_coef[0]*y_x_filter[0]+b_coef[1]*y_x_filter[1]+b_coef[2]*y_x_filter[2] //+b_coef[3]*y_x_filter[3] //+b_coef[4]*y_x_filter[4]+b_coef[5]*y_x_filter[5]+b_coef[6]*y_x_filter[6] |
JohanBeverini | 3:4c1180a712e3 | 209 | -(a_coef[1]*y_y_filter[1]+a_coef[2]*y_y_filter[2]); //+a_coef[3]*y_y_filter[3]); //+a_coef[4]*y_y_filter[4]+a_coef[5]*y_y_filter[5]+a_coef[6]*y_y_filter[6]); |
JohanBeverini | 3:4c1180a712e3 | 210 | gy_filtre=y_y_filter[0]; |
JohanBeverini | 3:4c1180a712e3 | 211 | |
JohanBeverini | 3:4c1180a712e3 | 212 | //z_x_filter[6]=z_x_filter[5]; z_x_filter[5]=z_x_filter[4]; z_x_filter[4]=z_x_filter[3]; |
JohanBeverini | 3:4c1180a712e3 | 213 | //z_x_filter[3]=z_x_filter[2]; |
JohanBeverini | 3:4c1180a712e3 | 214 | z_x_filter[2]=z_x_filter[1]; z_x_filter[1]=z_x_filter[0]; |
JohanBeverini | 3:4c1180a712e3 | 215 | z_x_filter[0]=gz; |
JohanBeverini | 3:4c1180a712e3 | 216 | //z_y_filter[6]=z_y_filter[5]; z_y_filter[5]=z_y_filter[4]; z_y_filter[4]=z_y_filter[3]; |
JohanBeverini | 3:4c1180a712e3 | 217 | //z_y_filter[3]=z_y_filter[2]; |
JohanBeverini | 3:4c1180a712e3 | 218 | z_y_filter[2]=z_y_filter[1]; z_y_filter[1]=z_y_filter[0]; |
JohanBeverini | 3:4c1180a712e3 | 219 | z_y_filter[0]=b_coef[0]*z_x_filter[0]+b_coef[1]*z_x_filter[1]+b_coef[2]*z_x_filter[2] //+b_coef[3]*z_x_filter[3] //+b_coef[4]*z_x_filter[4]+b_coef[5]*z_x_filter[5]+b_coef[6]*z_x_filter[6] |
JohanBeverini | 3:4c1180a712e3 | 220 | -(a_coef[1]*z_y_filter[1]+a_coef[2]*z_y_filter[2]); //+a_coef[3]*z_y_filter[3]); //+a_coef[4]*z_y_filter[4]+a_coef[5]*z_y_filter[5]+a_coef[6]*z_y_filter[6]); |
JohanBeverini | 3:4c1180a712e3 | 221 | gz_filtre=z_y_filter[0]; |
JohanBeverini | 3:4c1180a712e3 | 222 | |
JohanBeverini | 3:4c1180a712e3 | 223 | ////////filtre PB 100Hz / PH 1Hz |
JohanBeverini | 3:4c1180a712e3 | 224 | //x_x_filter[6]=x_x_filter[5]; x_x_filter[5]=x_x_filter[4]; x_x_filter[4]=x_x_filter[3]; |
JohanBeverini | 3:4c1180a712e3 | 225 | //x_x_filter_ph[3]=x_x_filter_ph[2]; |
JohanBeverini | 3:4c1180a712e3 | 226 | x_x_filter_ph[2]=x_x_filter_ph[1]; x_x_filter_ph[1]=x_x_filter_ph[0]; |
JohanBeverini | 3:4c1180a712e3 | 227 | x_x_filter_ph[0]=gx_filtre; |
JohanBeverini | 3:4c1180a712e3 | 228 | //x_y_filter[6]=x_y_filter[5]; x_y_filter[5]=x_y_filter[4]; x_y_filter[4]=x_y_filter[3]; |
JohanBeverini | 3:4c1180a712e3 | 229 | //x_y_filter_ph[3]=x_y_filter_ph[2]; |
JohanBeverini | 3:4c1180a712e3 | 230 | x_y_filter_ph[2]=x_y_filter_ph[1]; x_y_filter_ph[1]=x_y_filter_ph[0]; |
JohanBeverini | 3:4c1180a712e3 | 231 | x_y_filter_ph[0]=b_coef_ph[0]*x_x_filter_ph[0]+b_coef_ph[1]*x_x_filter_ph[1]+b_coef_ph[2]*x_x_filter_ph[2] //+b_coef_ph[3]*x_x_filter_ph[3] //+b_coef[4]*x_x_filter[4]+b_coef[5]*x_x_filter[5]+b_coef[6]*x_x_filter[6] |
JohanBeverini | 3:4c1180a712e3 | 232 | -(a_coef_ph[1]*x_y_filter_ph[1]+a_coef_ph[2]*x_y_filter_ph[2]); //+a_coef_ph[3]*x_y_filter_ph[3]); //+a_coef[4]*x_y_filter[4]+a_coef[5]*x_y_filter[5]+a_coef[6]*x_y_filter[6]); |
JohanBeverini | 3:4c1180a712e3 | 233 | gx_filtre=x_y_filter_ph[0]; |
JohanBeverini | 3:4c1180a712e3 | 234 | |
JohanBeverini | 3:4c1180a712e3 | 235 | //y_x_filter[6]=y_x_filter[5]; y_x_filter[5]=y_x_filter[4]; y_x_filter[4]=y_x_filter[3]; |
JohanBeverini | 3:4c1180a712e3 | 236 | //y_x_filter_ph[3]=y_x_filter_ph[2]; |
JohanBeverini | 3:4c1180a712e3 | 237 | y_x_filter_ph[2]=y_x_filter_ph[1]; y_x_filter_ph[1]=y_x_filter_ph[0]; |
JohanBeverini | 3:4c1180a712e3 | 238 | y_x_filter_ph[0]=gy_filtre; |
JohanBeverini | 3:4c1180a712e3 | 239 | //y_y_filter[6]=y_y_filter[5]; y_y_filter[5]=y_y_filter[4]; y_y_filter[4]=y_y_filter[3]; |
JohanBeverini | 3:4c1180a712e3 | 240 | //y_y_filter_ph[3]=y_y_filter_ph[2]; |
JohanBeverini | 3:4c1180a712e3 | 241 | y_y_filter_ph[2]=y_y_filter_ph[1]; y_y_filter_ph[1]=y_y_filter_ph[0]; |
JohanBeverini | 3:4c1180a712e3 | 242 | y_y_filter_ph[0]=b_coef_ph[0]*y_x_filter_ph[0]+b_coef_ph[1]*y_x_filter_ph[1]+b_coef_ph[2]*y_x_filter_ph[2] //+b_coef_ph[3]*y_x_filter_ph[3] //+b_coef[4]*y_x_filter[4]+b_coef[5]*y_x_filter[5]+b_coef[6]*y_x_filter[6] |
JohanBeverini | 3:4c1180a712e3 | 243 | -(a_coef_ph[1]*y_y_filter_ph[1]+a_coef_ph[2]*y_y_filter_ph[2]); //+a_coef_ph[3]*y_y_filter_ph[3]); //+a_coef[4]*y_y_filter[4]+a_coef[5]*y_y_filter[5]+a_coef[6]*y_y_filter[6]); |
JohanBeverini | 3:4c1180a712e3 | 244 | gy_filtre=y_y_filter_ph[0]; |
JohanBeverini | 3:4c1180a712e3 | 245 | |
JohanBeverini | 3:4c1180a712e3 | 246 | //z_x_filter[6]=z_x_filter[5]; z_x_filter[5]=z_x_filter[4]; z_x_filter[4]=z_x_filter[3]; |
JohanBeverini | 3:4c1180a712e3 | 247 | //z_x_filter_ph[3]=z_x_filter_ph[2]; |
JohanBeverini | 3:4c1180a712e3 | 248 | z_x_filter_ph[2]=z_x_filter_ph[1]; z_x_filter_ph[1]=z_x_filter_ph[0]; |
JohanBeverini | 3:4c1180a712e3 | 249 | z_x_filter_ph[0]=gz_filtre; |
JohanBeverini | 3:4c1180a712e3 | 250 | //z_y_filter[6]=z_y_filter[5]; z_y_filter[5]=z_y_filter[4]; z_y_filter[4]=z_y_filter[3]; |
JohanBeverini | 3:4c1180a712e3 | 251 | //z_y_filter_ph[3]=z_y_filter_ph[2]; |
JohanBeverini | 3:4c1180a712e3 | 252 | z_y_filter_ph[2]=z_y_filter_ph[1]; z_y_filter_ph[1]=z_y_filter_ph[0]; |
JohanBeverini | 3:4c1180a712e3 | 253 | z_y_filter_ph[0]=b_coef_ph[0]*z_x_filter_ph[0]+b_coef_ph[1]*z_x_filter_ph[1]+b_coef_ph[2]*z_x_filter_ph[2] //+b_coef_ph[3]*z_x_filter_ph[3] //+b_coef[4]*z_x_filter[4]+b_coef[5]*z_x_filter[5]+b_coef[6]*z_x_filter[6] |
JohanBeverini | 3:4c1180a712e3 | 254 | -(a_coef_ph[1]*z_y_filter_ph[1]+a_coef_ph[2]*z_y_filter_ph[2]); //+a_coef_ph[3]*z_y_filter_ph[3]); //+a_coef[4]*z_y_filter[4]+a_coef[5]*z_y_filter[5]+a_coef[6]*z_y_filter[6]); |
JohanBeverini | 3:4c1180a712e3 | 255 | gz_filtre=z_y_filter_ph[0]; |
JohanBeverini | 3:4c1180a712e3 | 256 | |
JohanBeverini | 3:4c1180a712e3 | 257 | |
JohanBeverini | 3:4c1180a712e3 | 258 | trapeze_x=deltat*((gx_filtre+gx_filtre2)/2.0f); |
JohanBeverini | 3:4c1180a712e3 | 259 | trapeze_y=deltat*((gy_filtre+gy_filtre2)/2.0f); |
JohanBeverini | 3:4c1180a712e3 | 260 | trapeze_z=deltat*((gz_filtre+gz_filtre2)/2.0f); |
JohanBeverini | 3:4c1180a712e3 | 261 | |
JohanBeverini | 3:4c1180a712e3 | 262 | gx_filtre2=gx_filtre; |
JohanBeverini | 3:4c1180a712e3 | 263 | gy_filtre2=gy_filtre; |
JohanBeverini | 3:4c1180a712e3 | 264 | gz_filtre2=gz_filtre; |
JohanBeverini | 3:4c1180a712e3 | 265 | |
JohanBeverini | 3:4c1180a712e3 | 266 | //calcule angle |
JohanBeverini | 3:4c1180a712e3 | 267 | alpha+=trapeze_x; |
JohanBeverini | 3:4c1180a712e3 | 268 | betaa+=trapeze_y; |
JohanBeverini | 3:4c1180a712e3 | 269 | gammaa+=trapeze_z; |
JohanBeverini | 3:4c1180a712e3 | 270 | |
JohanBeverini | 3:4c1180a712e3 | 271 | if(alpha>=360.0f){alpha-=360.0f;} |
JohanBeverini | 3:4c1180a712e3 | 272 | if(alpha<=-360.0f){alpha+=360.0f;} |
JohanBeverini | 3:4c1180a712e3 | 273 | if(betaa>=360.0f){betaa-=360.0f;} |
JohanBeverini | 3:4c1180a712e3 | 274 | if(betaa<=-360.0f){betaa+=360.0f;} |
JohanBeverini | 3:4c1180a712e3 | 275 | if(gammaa>=360.0f){gammaa-=360.0f;} |
JohanBeverini | 3:4c1180a712e3 | 276 | if(gammaa<=-360.0f){gammaa+=360.0f;} |
JohanBeverini | 3:4c1180a712e3 | 277 | |
JohanBeverini | 3:4c1180a712e3 | 278 | ANA1.write((alpha+500.0f)/1000.0f); |
JohanBeverini | 3:4c1180a712e3 | 279 | //ANA2.write(alpha/360.0f); |
onehorse | 0:65aa78c10981 | 280 | |
onehorse | 0:65aa78c10981 | 281 | // Serial print and/or display at 0.5 s rate independent of data rates |
onehorse | 0:65aa78c10981 | 282 | delt_t = t.read_ms() - count; |
JohanBeverini | 3:4c1180a712e3 | 283 | if (delt_t > 100) { // update LCD once per half-second independent of read rate |
onehorse | 1:cea9d83b8636 | 284 | |
onehorse | 1:cea9d83b8636 | 285 | pc.printf("ax = %f", 1000*ax); |
onehorse | 1:cea9d83b8636 | 286 | pc.printf(" ay = %f", 1000*ay); |
onehorse | 1:cea9d83b8636 | 287 | pc.printf(" az = %f mg\n\r", 1000*az); |
onehorse | 0:65aa78c10981 | 288 | |
onehorse | 1:cea9d83b8636 | 289 | pc.printf("gx = %f", gx); |
onehorse | 1:cea9d83b8636 | 290 | pc.printf(" gy = %f", gy); |
onehorse | 1:cea9d83b8636 | 291 | pc.printf(" gz = %f deg/s\n\r", gz); |
onehorse | 1:cea9d83b8636 | 292 | |
JohanBeverini | 3:4c1180a712e3 | 293 | // pc.printf("post filtre : gx = %f", gx_filtre2); |
JohanBeverini | 3:4c1180a712e3 | 294 | // pc.printf(" gy = %f", gy_filtre2); |
JohanBeverini | 3:4c1180a712e3 | 295 | // pc.printf(" gz = %f deg/s\n\r", gz_filtre2); |
JohanBeverini | 3:4c1180a712e3 | 296 | |
onehorse | 1:cea9d83b8636 | 297 | pc.printf(" temperature = %f C\n\r", temperature); |
onehorse | 0:65aa78c10981 | 298 | |
JohanBeverini | 3:4c1180a712e3 | 299 | // pc.printf("q0 = %f\n\r", q[0]); |
JohanBeverini | 3:4c1180a712e3 | 300 | // pc.printf("q1 = %f\n\r", q[1]); |
JohanBeverini | 3:4c1180a712e3 | 301 | // pc.printf("q2 = %f\n\r", q[2]); |
JohanBeverini | 3:4c1180a712e3 | 302 | // pc.printf("q3 = %f\n\r", q[3]); |
onehorse | 1:cea9d83b8636 | 303 | |
JohanBeverini | 3:4c1180a712e3 | 304 | //lcd.clear(); |
JohanBeverini | 3:4c1180a712e3 | 305 | //lcd.printString("MPU6050", 0, 0); |
JohanBeverini | 3:4c1180a712e3 | 306 | //lcd.printString("x y z", 0, 1); |
JohanBeverini | 3:4c1180a712e3 | 307 | //lcd.setXYAddress(0, 2); lcd.printChar((char)(1000*ax)); |
JohanBeverini | 3:4c1180a712e3 | 308 | //lcd.setXYAddress(20, 2); lcd.printChar((char)(1000*ay)); |
JohanBeverini | 3:4c1180a712e3 | 309 | //lcd.setXYAddress(40, 2); lcd.printChar((char)(1000*az)); lcd.printString("mg", 66, 2); |
onehorse | 1:cea9d83b8636 | 310 | |
onehorse | 0:65aa78c10981 | 311 | |
onehorse | 0:65aa78c10981 | 312 | // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. |
onehorse | 0:65aa78c10981 | 313 | // In this coordinate system, the positive z-axis is down toward Earth. |
onehorse | 0:65aa78c10981 | 314 | // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. |
onehorse | 0:65aa78c10981 | 315 | // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. |
onehorse | 0:65aa78c10981 | 316 | // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. |
onehorse | 0:65aa78c10981 | 317 | // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. |
onehorse | 0:65aa78c10981 | 318 | // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be |
onehorse | 0:65aa78c10981 | 319 | // applied in the correct order which for this configuration is yaw, pitch, and then roll. |
onehorse | 0:65aa78c10981 | 320 | // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. |
JohanBeverini | 3:4c1180a712e3 | 321 | //yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); |
JohanBeverini | 3:4c1180a712e3 | 322 | //pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); |
JohanBeverini | 3:4c1180a712e3 | 323 | //roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); |
JohanBeverini | 3:4c1180a712e3 | 324 | //pitch *= 180.0f / PI; |
JohanBeverini | 3:4c1180a712e3 | 325 | //yaw *= 180.0f / PI; |
JohanBeverini | 3:4c1180a712e3 | 326 | //roll *= 180.0f / PI; |
onehorse | 0:65aa78c10981 | 327 | |
onehorse | 1:cea9d83b8636 | 328 | // pc.printf("Yaw, Pitch, Roll: \n\r"); |
onehorse | 1:cea9d83b8636 | 329 | // pc.printf("%f", yaw); |
onehorse | 1:cea9d83b8636 | 330 | // pc.printf(", "); |
onehorse | 1:cea9d83b8636 | 331 | // pc.printf("%f", pitch); |
onehorse | 1:cea9d83b8636 | 332 | // pc.printf(", "); |
onehorse | 1:cea9d83b8636 | 333 | // pc.printf("%f\n\r", roll); |
onehorse | 1:cea9d83b8636 | 334 | // pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r"); |
onehorse | 0:65aa78c10981 | 335 | |
JohanBeverini | 3:4c1180a712e3 | 336 | //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll); |
JohanBeverini | 3:4c1180a712e3 | 337 | //pc.printf("average rate = %f\n\r", (float) sumCount/sum); |
JohanBeverini | 3:4c1180a712e3 | 338 | |
JohanBeverini | 3:4c1180a712e3 | 339 | //BT.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll); |
JohanBeverini | 3:4c1180a712e3 | 340 | //BT.printf("average rate = %f\n\r", (float) sumCount/sum); |
JohanBeverini | 3:4c1180a712e3 | 341 | |
JohanBeverini | 3:4c1180a712e3 | 342 | //alpha=yaw; |
JohanBeverini | 3:4c1180a712e3 | 343 | //betaa=pitch; |
JohanBeverini | 3:4c1180a712e3 | 344 | //gammaa=roll; |
JohanBeverini | 3:4c1180a712e3 | 345 | |
JohanBeverini | 3:4c1180a712e3 | 346 | pc.printf("delta = %f\n\r", (float) deltat); |
JohanBeverini | 3:4c1180a712e3 | 347 | // pc.printf("alpha, beta, gamma: %f %f %f\n\r", alpha, betaa, gammaa); |
JohanBeverini | 3:4c1180a712e3 | 348 | |
JohanBeverini | 3:4c1180a712e3 | 349 | axx=ax; |
JohanBeverini | 3:4c1180a712e3 | 350 | ayy=ay; |
JohanBeverini | 3:4c1180a712e3 | 351 | azz=az; |
JohanBeverini | 3:4c1180a712e3 | 352 | |
JohanBeverini | 3:4c1180a712e3 | 353 | ////////////////////////////////////////////////////////Matrice d'Euler(); |
JohanBeverini | 3:4c1180a712e3 | 354 | c = cos(alpha*PI/180.0f); s = sin(alpha*PI/180.0f); |
JohanBeverini | 3:4c1180a712e3 | 355 | a = cos(betaa*PI/180.0f); b = sin(betaa*PI/180.0f); |
JohanBeverini | 3:4c1180a712e3 | 356 | d = cos(gammaa*PI/180.0f); e = sin(gammaa*PI/180.0f); |
JohanBeverini | 3:4c1180a712e3 | 357 | |
JohanBeverini | 3:4c1180a712e3 | 358 | matrice[0][0] = e*a - e*c*b; |
JohanBeverini | 3:4c1180a712e3 | 359 | matrice[0][1] = (-d)*b - e*c*a; |
JohanBeverini | 3:4c1180a712e3 | 360 | matrice[0][2] = e*s; |
JohanBeverini | 3:4c1180a712e3 | 361 | matrice[1][0] = e*a + d*c*b; |
JohanBeverini | 3:4c1180a712e3 | 362 | matrice[1][1] = (-e)*b + d*c*a; |
JohanBeverini | 3:4c1180a712e3 | 363 | matrice[1][2] = (-d)*s; |
JohanBeverini | 3:4c1180a712e3 | 364 | matrice[2][0] = s*b; |
JohanBeverini | 3:4c1180a712e3 | 365 | matrice[2][1] = s*a; |
JohanBeverini | 3:4c1180a712e3 | 366 | matrice[2][2] = c; |
JohanBeverini | 3:4c1180a712e3 | 367 | |
JohanBeverini | 3:4c1180a712e3 | 368 | for(i=0; i<3; i++) |
JohanBeverini | 3:4c1180a712e3 | 369 | { |
JohanBeverini | 3:4c1180a712e3 | 370 | float temp = 0; |
JohanBeverini | 3:4c1180a712e3 | 371 | temp = axx * matrice[i][0] + ayy * matrice[i][1] + azz * matrice[i][2]; |
JohanBeverini | 3:4c1180a712e3 | 372 | resultat[i] = temp; |
JohanBeverini | 3:4c1180a712e3 | 373 | } |
JohanBeverini | 3:4c1180a712e3 | 374 | ////////////////////////////////////////////////////////// |
JohanBeverini | 3:4c1180a712e3 | 375 | |
JohanBeverini | 3:4c1180a712e3 | 376 | // if (first==true){ |
JohanBeverini | 3:4c1180a712e3 | 377 | // poid[0]=resultat[0]; |
JohanBeverini | 3:4c1180a712e3 | 378 | // poid[1]=resultat[1]; |
JohanBeverini | 3:4c1180a712e3 | 379 | // poid[2]=resultat[2]; |
JohanBeverini | 3:4c1180a712e3 | 380 | // first=false; |
JohanBeverini | 3:4c1180a712e3 | 381 | // } else { |
JohanBeverini | 3:4c1180a712e3 | 382 | // resultat[0]-=poid[0]; |
JohanBeverini | 3:4c1180a712e3 | 383 | // resultat[1]-=poid[1]; |
JohanBeverini | 3:4c1180a712e3 | 384 | // resultat[2]-=poid[2]; |
JohanBeverini | 3:4c1180a712e3 | 385 | // } |
JohanBeverini | 3:4c1180a712e3 | 386 | |
JohanBeverini | 3:4c1180a712e3 | 387 | // pc.printf("acceleration sans Euler : %f ; %f ; %f\n\r", axx, ayy, azz); |
JohanBeverini | 3:4c1180a712e3 | 388 | // pc.printf("acceleration avec Euler : %f ; %f ; %f\n\r", resultat[0], resultat[1], resultat[2]); |
JohanBeverini | 3:4c1180a712e3 | 389 | BT.printf("acceleration sans Euler : %f ; %f ; %f\n\r", axx, ayy, azz); |
JohanBeverini | 3:4c1180a712e3 | 390 | BT.printf("acceleration avec Euler : %f ; %f ; %f\n\r", resultat[0], resultat[1], resultat[2]); |
JohanBeverini | 3:4c1180a712e3 | 391 | |
onehorse | 1:cea9d83b8636 | 392 | |
onehorse | 1:cea9d83b8636 | 393 | myled= !myled; |
onehorse | 1:cea9d83b8636 | 394 | count = t.read_ms(); |
onehorse | 1:cea9d83b8636 | 395 | sum = 0; |
onehorse | 1:cea9d83b8636 | 396 | sumCount = 0; |
onehorse | 0:65aa78c10981 | 397 | } |
onehorse | 0:65aa78c10981 | 398 | } |
JohanBeverini | 3:4c1180a712e3 | 399 | if (BT.readable()) { |
JohanBeverini | 3:4c1180a712e3 | 400 | char c = BT.getc(); |
JohanBeverini | 3:4c1180a712e3 | 401 | if(c == 'a') { |
JohanBeverini | 3:4c1180a712e3 | 402 | BT.printf("\nOK\n"); |
JohanBeverini | 3:4c1180a712e3 | 403 | } |
JohanBeverini | 3:4c1180a712e3 | 404 | } |
JohanBeverini | 3:4c1180a712e3 | 405 | } |
onehorse | 1:cea9d83b8636 | 406 | |
JohanBeverini | 3:4c1180a712e3 | 407 | } |
JohanBeverini | 3:4c1180a712e3 | 408 | |
JohanBeverini | 3:4c1180a712e3 | 409 | |
JohanBeverini | 3:4c1180a712e3 | 410 |