João Pereira
/
Gyroscope
Alteração de partes não utilizadas
main.cpp
- Committer:
- einsteingustavo
- Date:
- 2018-09-29
- Revision:
- 0:cd56e3a52a36
- Child:
- 2:2dfe6101b283
File content as of revision 0:cd56e3a52a36:
#include "stm32f103c8t6.h" #include "Kalman.h" #include "mbed.h" #include "LSM6DS3.h" //#include "FXOS8700CQ.h" #define RESTRICT_PITCH #define RAD_TO_DEG 180/3.14 Kalman kalmanX; // Create the Kalman instances Kalman kalmanY; Serial pc(PA_9, PA_10); // tx, rx LSM6DS3 LSM6DS3(PB_7, PB_6); //FXOS8700CQ fxos(PTE25,PTE24); //Data fxos_acc; Timer t; //AnalogIn pot0(A0), pot1(A1), pot2(A2), pot3(A3); unsigned long int start_time = 0, end_time = 0, t1; uint16_t adc_value; double gyroXangle, gyroYangle; // Angle calculate using the gyro only double compAngleX, compAngleY; // Calculated angle using a complementary filter double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter void gyro_getdata(); int main() { t.start(); pc.baud(9600); // pc.printf("before\n"); LSM6DS3.begin(LSM6DS3.G_SCALE_245DPS, LSM6DS3.A_SCALE_2G, LSM6DS3.G_ODR_208, LSM6DS3.A_ODR_208); // pc.printf("after\n"); // LSM6DS3.begin(); // fxos.init(); LSM6DS3.readAccel(); LSM6DS3.readGyro(); #ifdef RESTRICT_PITCH double roll = atan2(LSM6DS3.ay, LSM6DS3.az) * RAD_TO_DEG; double pitch = atan(-LSM6DS3.ax / sqrt(LSM6DS3.ay * LSM6DS3.ay + LSM6DS3.az * LSM6DS3.az)) * RAD_TO_DEG; #else // Eq. 28 and 29 double roll = atan(LSM6DS3.ay / sqrt(LSM6DS3.ax * LSM6DS3.ax + LSM6DS3.ax * LSM6DS3.ax)) * RAD_TO_DEG; double pitch = atan2(-LSM6DS3.ax, LSM6DS3.ax) * RAD_TO_DEG; #endif kalmanX.setAngle(roll); // Set starting angle kalmanY.setAngle(pitch); gyroXangle = roll; gyroYangle = pitch; compAngleX = roll; compAngleY = pitch; t1 = t.read_us(); while(1) { start_time = t.read_us(); //read Accel & Gyro // fxos_acc = fxos.get_values(); LSM6DS3.readAccel(); LSM6DS3.readGyro(); end_time = t.read_us(); // pc.printf("\nPass time: %d\n", end_time - start_time); //serial send Accel (board) // pc.printf("BoardAccelerX[%f]\n",fxos_acc.ax); // pc.printf("BoardAccelerY[%f]\n",fxos_acc.ay); // pc.printf("BoardAccelerZ[%f]\n",fxos_acc.az); //serial send Accel (lsm6ds33) // pc.printf("AccelerX[%f]\n",LSM6DS3.ax); // pc.printf("AccelerY[%f]\n",LSM6DS3.ay); // pc.printf("AccelerZ[%f]\n",LSM6DS3.az); //serial send Gyro // pc.printf("GyroX[%f]\n",LSM6DS3.gx); // pc.printf("GyroY[%f]\n",LSM6DS3.gy); // pc.printf("GyroZ[%f]\n",LSM6DS3.gz); wait(1.0f); gyro_getdata(); pc.printf("\n"); }} void gyro_getdata(){ LSM6DS3.readAccel(); LSM6DS3.readGyro(); double dt = (double)(t.read_us() - t1) / 1000000; // Calculate delta time t1 = t.read_us(); #ifdef RESTRICT_PITCH double roll = atan2(LSM6DS3.ay, LSM6DS3.az) * RAD_TO_DEG; double pitch = atan(-LSM6DS3.ax / sqrt(LSM6DS3.ay * LSM6DS3.ay + LSM6DS3.az * LSM6DS3.az)) * RAD_TO_DEG; #else // Eq. 28 and 29 double roll = atan(LSM6DS3.ay / sqrt(LSM6DS3.ax * LSM6DS3.ax + LSM6DS3.ax * LSM6DS3.ax)) * RAD_TO_DEG; double pitch = atan2(-LSM6DS3.ax, LSM6DS3.ax) * RAD_TO_DEG; #endif double gyroXrate = LSM6DS3.gx / 131.0; // Convert to deg/s double gyroYrate = LSM6DS3.gy / 131.0; // Convert to deg/s #ifdef RESTRICT_PITCH // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) { kalmanX.setAngle(roll); compAngleX = roll; kalAngleX = roll; gyroXangle = roll; } else kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter if (abs(kalAngleX) > 90) gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); #else // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) { kalmanY.setAngle(pitch); compAngleY = pitch; kalAngleY = pitch; gyroYangle = pitch; } else kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter if (abs(kalAngleY) > 90) gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter #endif gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter gyroYangle += gyroYrate * dt; gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate gyroYangle += kalmanY.getRate() * dt; compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch; // Reset the gyro angle when it has drifted too much if (gyroXangle < -180 || gyroXangle > 180) gyroXangle = kalAngleX; if (gyroYangle < -180 || gyroYangle > 180) gyroYangle = kalAngleY; pc.printf("%.2f",roll); pc.printf("\n"); pc.printf("%.2f",pitch); pc.printf("\n"); wait_ms(500); }