¡¡¡Disculpa la demora!!!
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define GYRORATE_SEL 2000
#define ACCELRATE_SEL 16
MPU6050 accelgyro;
Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;
/* IMU Data */
int16_t ax, ay, az;
int16_t tempRaw;
int16_t gx, gy, gz;
float temp; // Temperature
float kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter
uint32_t timer;
float fXg = 0;
float fYg = 0;
float fZg = 0;
float pitch, roll;
float Xg, Yg, Zg;
float gXg, gYg, gZg;
float gyroXrate, gyroYrate, gyroZrate;
void setup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
accelgyro.initialize();
char text[256];
sprintf(text, "%s", accelgyro.testConnection() ? "MPU6050 successful" : "MPU6050 failed");
Serial.println(text);
//************ COMPASS CODE ****************
accelgyro.setI2CBypassEnabled(true); // HMC5883L enable
compass_x_offset = 122.17;
compass_y_offset = 230.08;
compass_z_offset = 389.85;
compass_x_gainError = 1.12;
compass_y_gainError = 1.13;
compass_z_gainError = 1.03;
//accelgyro.setXAccelOffset(-1025);
//accelgyro.setYAccelOffset(1244);//1283
//accelgyro.setZAccelOffset(1277);
//accelgyro.setXGyroOffset(114);
//accelgyro.setYGyroOffset(64);
//accelgyro.setZGyroOffset(-1);
sprintf(text, "Setting %d DEG/s", GYRORATE_SEL);
Serial.print(text);
switch (GYRORATE_SEL) {
case 250:
accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
break;
case 500:
accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_500);
break;
case 1000:
accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_1000);
break;
case 2000:
accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
break;
}
Serial.println("Done!");
sprintf(text, "Setting %d Gs", ACCELRATE_SEL);
Serial.print(text);
switch (ACCELRATE_SEL) {
case 2:
accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
break;
case 4:
accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_4);
break;
case 8:
accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);
break;
case 16:
accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_16);
break;
}
Serial.println("Done!");
// reading MPU for the first time
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//Low Pass Filter---> this actually smooths the readings
Xg = ax * (ACCELRATE_SEL / pow(2, 15));
Yg = ay * (ACCELRATE_SEL / pow(2, 15));
Zg = -az * (ACCELRATE_SEL / pow(2, 15));
gXg = gx * (ACCELRATE_SEL / pow(2, 15));
gYg = gy * (ACCELRATE_SEL / pow(2, 15));
gZg = gz * (ACCELRATE_SEL / pow(2, 15));
fXg = Xg * alpha + (fXg * (1.0 - alpha));
fYg = Yg * alpha + (fYg * (1.0 - alpha));
fZg = Zg * alpha + (fZg * (1.0 - alpha));
pitch = atan2(fYg, sqrt(fXg * fXg + fZg * fZg)) * RAD_TO_DEG;
roll = atan2(fXg, fZg) * RAD_TO_DEG;
timer = millis();
}
void loop() {
Get_Values();
char text[256];
sprintf(text,"Pitch: %03d Roll: %03d Kalman Pitch: %03d Kalman Roll: %03d", pitch, roll, kalAngleX, kalAngleY);
Serial.println(text);
}
void Get_Values()
{
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
//Low Pass Filter
Xg = ax * (ACCELRATE_SEL / pow(2, 15));
Yg = ay * (ACCELRATE_SEL / pow(2, 15));
Zg = -az * (ACCELRATE_SEL / pow(2, 15));
gXg = gx * (ACCELRATE_SEL / pow(2, 15));
gYg = gy * (ACCELRATE_SEL / pow(2, 15));
gZg = gz * (ACCELRATE_SEL / pow(2, 15));
fXg = Xg * alpha + (fXg * (1.0 - alpha));
fYg = Yg * alpha + (fYg * (1.0 - alpha));
fZg = Zg * alpha + (fZg * (1.0 - alpha));
pitch = atan2(fYg, sqrt(fXg * fXg + fZg * fZg)) * RAD_TO_DEG;
roll = 180.00 - atan2(fXg, fZg) * RAD_TO_DEG;
gyroXrate = (float)gx / (GYRORATE_SEL == 250 ? 131.0 : GYRORATE_SEL == 500 ? 65.5 : GYRORATE_SEL == 1000 ? 32.8 : 16.4);
gyroYrate = -(float)gy / (GYRORATE_SEL == 250 ? 131.0 : GYRORATE_SEL == 500 ? 65.5 : GYRORATE_SEL == 1000 ? 32.8 : 16.4);
gyroZrate = (float)gz / (GYRORATE_SEL == 250 ? 131.0 : GYRORATE_SEL == 500 ? 65.5 : GYRORATE_SEL == 1000 ? 32.8 : 16.4);
kalAngleX = kalmanX.getAngle(pitch, gyroXrate, (float)(micros() - timer) / 1000000); // - float(180.00) Calculate the angle using a Kalman filter
kalAngleY = kalmanY.getAngle(roll , gyroYrate, (float)(micros() - timer) / 1000000); //- float(180.00)
timer = millis();
temp = ((float)tempRaw + 12412.0) / 340.0;
}