![](/media/cache/profiles/27661447_1561692407200368_647610243_n.jpg.50x50_q85.jpg)
Gyroscope with kalman
Fork of Gyroscope by
main.cpp@4:55f97d30c14f, 2018-10-11 (annotated)
- Committer:
- danilloaguiar
- Date:
- Thu Oct 11 01:54:08 2018 +0000
- Revision:
- 4:55f97d30c14f
- Parent:
- 2:2dfe6101b283
Gyroscope with kalman
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
einsteingustavo | 0:cd56e3a52a36 | 1 | #include "stm32f103c8t6.h" |
einsteingustavo | 0:cd56e3a52a36 | 2 | #include "Kalman.h" |
einsteingustavo | 0:cd56e3a52a36 | 3 | #include "mbed.h" |
einsteingustavo | 0:cd56e3a52a36 | 4 | #include "LSM6DS3.h" |
einsteingustavo | 0:cd56e3a52a36 | 5 | //#include "FXOS8700CQ.h" |
einsteingustavo | 0:cd56e3a52a36 | 6 | #define RESTRICT_PITCH |
einsteingustavo | 0:cd56e3a52a36 | 7 | #define RAD_TO_DEG 180/3.14 |
einsteingustavo | 0:cd56e3a52a36 | 8 | Kalman kalmanX; // Create the Kalman instances |
einsteingustavo | 0:cd56e3a52a36 | 9 | Kalman kalmanY; |
danilloaguiar | 4:55f97d30c14f | 10 | Serial pc(PA_2, PA_3); // tx, rx |
einsteingustavo | 0:cd56e3a52a36 | 11 | LSM6DS3 LSM6DS3(PB_7, PB_6); |
einsteingustavo | 0:cd56e3a52a36 | 12 | //FXOS8700CQ fxos(PTE25,PTE24); |
einsteingustavo | 0:cd56e3a52a36 | 13 | //Data fxos_acc; |
einsteingustavo | 0:cd56e3a52a36 | 14 | Timer t; |
einsteingustavo | 0:cd56e3a52a36 | 15 | //AnalogIn pot0(A0), pot1(A1), pot2(A2), pot3(A3); |
einsteingustavo | 0:cd56e3a52a36 | 16 | |
einsteingustavo | 0:cd56e3a52a36 | 17 | unsigned long int start_time = 0, end_time = 0, t1; |
einsteingustavo | 0:cd56e3a52a36 | 18 | uint16_t adc_value; |
einsteingustavo | 0:cd56e3a52a36 | 19 | |
einsteingustavo | 0:cd56e3a52a36 | 20 | double gyroXangle, gyroYangle; // Angle calculate using the gyro only |
joaomazza | 2:2dfe6101b283 | 21 | //double compAngleX, compAngleY; // Calculated angle using a complementary filter |
einsteingustavo | 0:cd56e3a52a36 | 22 | double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter |
einsteingustavo | 0:cd56e3a52a36 | 23 | |
einsteingustavo | 0:cd56e3a52a36 | 24 | void gyro_getdata(); |
einsteingustavo | 0:cd56e3a52a36 | 25 | |
einsteingustavo | 0:cd56e3a52a36 | 26 | int main() { |
einsteingustavo | 0:cd56e3a52a36 | 27 | t.start(); |
einsteingustavo | 0:cd56e3a52a36 | 28 | pc.baud(9600); |
einsteingustavo | 0:cd56e3a52a36 | 29 | // pc.printf("before\n"); |
einsteingustavo | 0:cd56e3a52a36 | 30 | LSM6DS3.begin(LSM6DS3.G_SCALE_245DPS, LSM6DS3.A_SCALE_2G, LSM6DS3.G_ODR_208, LSM6DS3.A_ODR_208); |
einsteingustavo | 0:cd56e3a52a36 | 31 | // pc.printf("after\n"); |
einsteingustavo | 0:cd56e3a52a36 | 32 | // LSM6DS3.begin(); |
einsteingustavo | 0:cd56e3a52a36 | 33 | // fxos.init(); |
einsteingustavo | 0:cd56e3a52a36 | 34 | LSM6DS3.readAccel(); |
einsteingustavo | 0:cd56e3a52a36 | 35 | LSM6DS3.readGyro(); |
einsteingustavo | 0:cd56e3a52a36 | 36 | #ifdef RESTRICT_PITCH |
einsteingustavo | 0:cd56e3a52a36 | 37 | double roll = atan2(LSM6DS3.ay, LSM6DS3.az) * RAD_TO_DEG; |
einsteingustavo | 0:cd56e3a52a36 | 38 | double pitch = atan(-LSM6DS3.ax / sqrt(LSM6DS3.ay * LSM6DS3.ay + LSM6DS3.az * LSM6DS3.az)) * RAD_TO_DEG; |
einsteingustavo | 0:cd56e3a52a36 | 39 | #else // Eq. 28 and 29 |
einsteingustavo | 0:cd56e3a52a36 | 40 | double roll = atan(LSM6DS3.ay / sqrt(LSM6DS3.ax * LSM6DS3.ax + LSM6DS3.ax * LSM6DS3.ax)) * RAD_TO_DEG; |
einsteingustavo | 0:cd56e3a52a36 | 41 | double pitch = atan2(-LSM6DS3.ax, LSM6DS3.ax) * RAD_TO_DEG; |
einsteingustavo | 0:cd56e3a52a36 | 42 | #endif |
einsteingustavo | 0:cd56e3a52a36 | 43 | kalmanX.setAngle(roll); // Set starting angle |
einsteingustavo | 0:cd56e3a52a36 | 44 | kalmanY.setAngle(pitch); |
einsteingustavo | 0:cd56e3a52a36 | 45 | gyroXangle = roll; |
einsteingustavo | 0:cd56e3a52a36 | 46 | gyroYangle = pitch; |
joaomazza | 2:2dfe6101b283 | 47 | // compAngleX = roll; |
joaomazza | 2:2dfe6101b283 | 48 | // compAngleY = pitch; |
einsteingustavo | 0:cd56e3a52a36 | 49 | t1 = t.read_us(); |
einsteingustavo | 0:cd56e3a52a36 | 50 | while(1) |
einsteingustavo | 0:cd56e3a52a36 | 51 | { |
einsteingustavo | 0:cd56e3a52a36 | 52 | start_time = t.read_us(); |
einsteingustavo | 0:cd56e3a52a36 | 53 | //read Accel & Gyro |
einsteingustavo | 0:cd56e3a52a36 | 54 | // fxos_acc = fxos.get_values(); |
einsteingustavo | 0:cd56e3a52a36 | 55 | LSM6DS3.readAccel(); |
einsteingustavo | 0:cd56e3a52a36 | 56 | LSM6DS3.readGyro(); |
einsteingustavo | 0:cd56e3a52a36 | 57 | end_time = t.read_us(); |
einsteingustavo | 0:cd56e3a52a36 | 58 | // pc.printf("\nPass time: %d\n", end_time - start_time); |
einsteingustavo | 0:cd56e3a52a36 | 59 | //serial send Accel (board) |
einsteingustavo | 0:cd56e3a52a36 | 60 | // pc.printf("BoardAccelerX[%f]\n",fxos_acc.ax); |
einsteingustavo | 0:cd56e3a52a36 | 61 | // pc.printf("BoardAccelerY[%f]\n",fxos_acc.ay); |
einsteingustavo | 0:cd56e3a52a36 | 62 | // pc.printf("BoardAccelerZ[%f]\n",fxos_acc.az); |
einsteingustavo | 0:cd56e3a52a36 | 63 | //serial send Accel (lsm6ds33) |
einsteingustavo | 0:cd56e3a52a36 | 64 | // pc.printf("AccelerX[%f]\n",LSM6DS3.ax); |
einsteingustavo | 0:cd56e3a52a36 | 65 | // pc.printf("AccelerY[%f]\n",LSM6DS3.ay); |
einsteingustavo | 0:cd56e3a52a36 | 66 | // pc.printf("AccelerZ[%f]\n",LSM6DS3.az); |
einsteingustavo | 0:cd56e3a52a36 | 67 | //serial send Gyro |
einsteingustavo | 0:cd56e3a52a36 | 68 | // pc.printf("GyroX[%f]\n",LSM6DS3.gx); |
einsteingustavo | 0:cd56e3a52a36 | 69 | // pc.printf("GyroY[%f]\n",LSM6DS3.gy); |
einsteingustavo | 0:cd56e3a52a36 | 70 | // pc.printf("GyroZ[%f]\n",LSM6DS3.gz); |
einsteingustavo | 0:cd56e3a52a36 | 71 | wait(1.0f); |
einsteingustavo | 0:cd56e3a52a36 | 72 | gyro_getdata(); |
einsteingustavo | 0:cd56e3a52a36 | 73 | pc.printf("\n"); |
einsteingustavo | 0:cd56e3a52a36 | 74 | }} |
einsteingustavo | 0:cd56e3a52a36 | 75 | |
einsteingustavo | 0:cd56e3a52a36 | 76 | void gyro_getdata(){ |
einsteingustavo | 0:cd56e3a52a36 | 77 | LSM6DS3.readAccel(); |
einsteingustavo | 0:cd56e3a52a36 | 78 | LSM6DS3.readGyro(); |
einsteingustavo | 0:cd56e3a52a36 | 79 | double dt = (double)(t.read_us() - t1) / 1000000; // Calculate delta time |
einsteingustavo | 0:cd56e3a52a36 | 80 | t1 = t.read_us(); |
einsteingustavo | 0:cd56e3a52a36 | 81 | #ifdef RESTRICT_PITCH |
einsteingustavo | 0:cd56e3a52a36 | 82 | double roll = atan2(LSM6DS3.ay, LSM6DS3.az) * RAD_TO_DEG; |
einsteingustavo | 0:cd56e3a52a36 | 83 | double pitch = atan(-LSM6DS3.ax / sqrt(LSM6DS3.ay * LSM6DS3.ay + LSM6DS3.az * LSM6DS3.az)) * RAD_TO_DEG; |
einsteingustavo | 0:cd56e3a52a36 | 84 | #else // Eq. 28 and 29 |
einsteingustavo | 0:cd56e3a52a36 | 85 | double roll = atan(LSM6DS3.ay / sqrt(LSM6DS3.ax * LSM6DS3.ax + LSM6DS3.ax * LSM6DS3.ax)) * RAD_TO_DEG; |
einsteingustavo | 0:cd56e3a52a36 | 86 | double pitch = atan2(-LSM6DS3.ax, LSM6DS3.ax) * RAD_TO_DEG; |
einsteingustavo | 0:cd56e3a52a36 | 87 | #endif |
einsteingustavo | 0:cd56e3a52a36 | 88 | double gyroXrate = LSM6DS3.gx / 131.0; // Convert to deg/s |
einsteingustavo | 0:cd56e3a52a36 | 89 | double gyroYrate = LSM6DS3.gy / 131.0; // Convert to deg/s |
einsteingustavo | 0:cd56e3a52a36 | 90 | |
einsteingustavo | 0:cd56e3a52a36 | 91 | #ifdef RESTRICT_PITCH |
einsteingustavo | 0:cd56e3a52a36 | 92 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees |
einsteingustavo | 0:cd56e3a52a36 | 93 | if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { |
einsteingustavo | 0:cd56e3a52a36 | 94 | kalmanX.setAngle(roll); |
joaomazza | 2:2dfe6101b283 | 95 | //compAngleX = roll; |
einsteingustavo | 0:cd56e3a52a36 | 96 | kalAngleX = roll; |
einsteingustavo | 0:cd56e3a52a36 | 97 | gyroXangle = roll; |
einsteingustavo | 0:cd56e3a52a36 | 98 | } else |
einsteingustavo | 0:cd56e3a52a36 | 99 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter |
einsteingustavo | 0:cd56e3a52a36 | 100 | |
einsteingustavo | 0:cd56e3a52a36 | 101 | if (abs(kalAngleX) > 90) |
einsteingustavo | 0:cd56e3a52a36 | 102 | gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading |
einsteingustavo | 0:cd56e3a52a36 | 103 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); |
einsteingustavo | 0:cd56e3a52a36 | 104 | #else |
einsteingustavo | 0:cd56e3a52a36 | 105 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees |
einsteingustavo | 0:cd56e3a52a36 | 106 | if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) { |
einsteingustavo | 0:cd56e3a52a36 | 107 | kalmanY.setAngle(pitch); |
joaomazza | 2:2dfe6101b283 | 108 | //compAngleY = pitch; |
einsteingustavo | 0:cd56e3a52a36 | 109 | kalAngleY = pitch; |
einsteingustavo | 0:cd56e3a52a36 | 110 | gyroYangle = pitch; |
einsteingustavo | 0:cd56e3a52a36 | 111 | } else |
einsteingustavo | 0:cd56e3a52a36 | 112 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter |
einsteingustavo | 0:cd56e3a52a36 | 113 | |
einsteingustavo | 0:cd56e3a52a36 | 114 | if (abs(kalAngleY) > 90) |
einsteingustavo | 0:cd56e3a52a36 | 115 | gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading |
einsteingustavo | 0:cd56e3a52a36 | 116 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter |
einsteingustavo | 0:cd56e3a52a36 | 117 | #endif |
einsteingustavo | 0:cd56e3a52a36 | 118 | |
einsteingustavo | 0:cd56e3a52a36 | 119 | gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter |
einsteingustavo | 0:cd56e3a52a36 | 120 | gyroYangle += gyroYrate * dt; |
einsteingustavo | 0:cd56e3a52a36 | 121 | gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate |
einsteingustavo | 0:cd56e3a52a36 | 122 | gyroYangle += kalmanY.getRate() * dt; |
einsteingustavo | 0:cd56e3a52a36 | 123 | |
joaomazza | 2:2dfe6101b283 | 124 | //compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter |
joaomazza | 2:2dfe6101b283 | 125 | //compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; |
einsteingustavo | 0:cd56e3a52a36 | 126 | |
einsteingustavo | 0:cd56e3a52a36 | 127 | // Reset the gyro angle when it has drifted too much |
einsteingustavo | 0:cd56e3a52a36 | 128 | if (gyroXangle < -180 || gyroXangle > 180) |
einsteingustavo | 0:cd56e3a52a36 | 129 | gyroXangle = kalAngleX; |
einsteingustavo | 0:cd56e3a52a36 | 130 | if (gyroYangle < -180 || gyroYangle > 180) |
einsteingustavo | 0:cd56e3a52a36 | 131 | gyroYangle = kalAngleY; |
einsteingustavo | 0:cd56e3a52a36 | 132 | pc.printf("%.2f",roll); |
einsteingustavo | 0:cd56e3a52a36 | 133 | pc.printf("\n"); |
einsteingustavo | 0:cd56e3a52a36 | 134 | pc.printf("%.2f",pitch); |
einsteingustavo | 0:cd56e3a52a36 | 135 | pc.printf("\n"); |
danilloaguiar | 4:55f97d30c14f | 136 | wait_ms(100); |
einsteingustavo | 0:cd56e3a52a36 | 137 | } |