João Pereira / Mbed 2 deprecated Gyroscope

Alteração de partes não utilizadas

Dependencies:   LSM6DS3 mbed

main.cpp@3:78402ec2d9c0, 2018-10-03 (annotated)

Committer:
joaomazza
Date:
Wed Oct 03 01:06:06 2018 +0000
Revision:
3:78402ec2d9c0
Parent:
2:2dfe6101b283 
Teste

Who changed what in which revision?

UserRevisionLine numberNew 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;
einsteingustavo 0:cd56e3a52a36 10 Serial pc(PA_9, PA_10); // 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");
einsteingustavo 0:cd56e3a52a36 136 wait_ms(500);
einsteingustavo 0:cd56e3a52a36 137 }