void print_calculated_angular(){
Serial.print("gyro_angle:"); Serial.print(gyro_angle); Serial.print(" ");
Serial.print("range: ");Serial.print(mpu.getFullScaleGyroRange());
Serial.print("mag_angle:"); Serial.print(mag_angle); Serial.print(" ");
Serial.print("mag_angle_2:"); Serial.print(mag_angle_2); Serial.print(" ");
// Serial.print("current_angle:"); Serial.print(current_angle); Serial.print(" ");
}
int main()
{
MPU6050 *mpu = new MPU6050();
mpu->setDebug(true);
mpu->reset();
if (mpu->whoAmI())
{
printf("WhoAmI was okay\n");
// i2c bypass enabled
mpu->setBypassEnable(true);
printf("Set and Get BypassEnable to true - %s\n", mpu->getBypassEnable() ? "SUCCESS" : "FAILED");
mpu->setBypassEnable(false);
printf("Set and Get BypassEnable to false - %s\n", !mpu->getBypassEnable() ? "SUCCESS" : "FAILED");
// gyro ranges
mpu->setFullScaleGyroRange(fullScaleGyroRange::FS_GYRO_250DEG_S);
printf("Set and Get FullScaleGyroRange to 250deg/sec - %s\n", (mpu->getFullScaleGyroRange() == fullScaleGyroRange::FS_GYRO_250DEG_S) ? "SUCCESS" : "FAILED");
mpu->setFullScaleGyroRange(fullScaleGyroRange::FS_GYRO_500DEG_S);
printf("Set and Get FullScaleGyroRange to 500deg/sec - %s\n", (mpu->getFullScaleGyroRange() == fullScaleGyroRange::FS_GYRO_500DEG_S) ? "SUCCESS" : "FAILED");
mpu->setFullScaleGyroRange(fullScaleGyroRange::FS_GYRO_1000DEG_S);
printf("Set and Get FullScaleGyroRange to 1000deg/sec - %s\n", (mpu->getFullScaleGyroRange() == fullScaleGyroRange::FS_GYRO_1000DEG_S) ? "SUCCESS" : "FAILED");
mpu->setFullScaleGyroRange(fullScaleGyroRange::FS_GYRO_2000DEG_S);
printf("Set and Get FullScaleGyroRange to 2000deg/sec - %s\n", (mpu->getFullScaleGyroRange() == fullScaleGyroRange::FS_GYRO_2000DEG_S) ? "SUCCESS" : "FAILED");
// accelerometer ranges
mpu->setFullScaleAccRange(fullScaleAccRange::FS_ACCL_2G);
printf("Set and Get FullScaleAccRange to 2G - %s\n", (mpu->getFullScaleAccRange() == fullScaleAccRange::FS_ACCL_2G) ? "SUCCESS" : "FAILED");
mpu->setFullScaleAccRange(fullScaleAccRange::FS_ACCL_4G);
printf("Set and Get FullScaleAccRange to 4G - %s\n", (mpu->getFullScaleAccRange() == fullScaleAccRange::FS_ACCL_4G) ? "SUCCESS" : "FAILED");
mpu->setFullScaleAccRange(fullScaleAccRange::FS_ACCL_8G);
printf("Set and Get FullScaleAccRange to 8G - %s\n", (mpu->getFullScaleAccRange() == fullScaleAccRange::FS_ACCL_8G) ? "SUCCESS" : "FAILED");
mpu->setFullScaleAccRange(fullScaleAccRange::FS_ACCL_16G);
printf("Set and Get FullScaleAccRange to 16G - %s\n", (mpu->getFullScaleAccRange() == fullScaleAccRange::FS_ACCL_16G) ? "SUCCESS" : "FAILED");
return 1;
}
return 0;
}
bool IntegrateGyro()
{
// Set the full scale range of the gyro
uint8_t FS_SEL = 0;
int16_t accX, accY, accZ;
int16_t gyroX, gyroY, gyroZ;
mpu.getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ); //Set Starting angles
unsigned long now = millis();
float dt =(now - Before)/1000.0;
Before = now;
//mpu.setFullScaleGyroRange(FS_SEL);
// get default full scale value of gyro - may have changed from default
// function call returns values between 0 and 3
uint8_t READ_FS_SEL = mpu.getFullScaleGyroRange();
// Serial.print("FS_SEL = ");
// Serial.println(READ_FS_SEL);
GYRO_FACTOR = 131.0/(READ_FS_SEL + 1);
// get default full scale value of accelerometer - may not be default value.
// Accelerometer scale factor doesn't reall matter as it divides out
uint8_t READ_AFS_SEL = mpu.getFullScaleAccelRange();
// Serial.print("AFS_SEL = ");
// Serial.println(READ_AFS_SEL);
//ACCEL_FACTOR = 16384.0/(AFS_SEL + 1);
// Remove offsets and scale gyro data
float fgyroX, fgyroY, fgyroZ;
fgyroX = (gyroX - gXOffset)/GYRO_FACTOR;
fgyroY = -(gyroY - gYOffset)/GYRO_FACTOR;
fgyroZ = -(gyroZ - gZOffset)/GYRO_FACTOR;
accX = accX; // - base_x_accel;
accY = accY; // - base_y_accel;
accZ = accZ; // - base_z_accel;
const double Q_angle = 0.001;
const double Q_gyroBias = 0.003;
const double R_angle = 0.03;
AccelAngleY = atan2(accX, sqrt(pow(accY,2) + pow(accZ,2)))*RADIANS_TO_DEGREES;
AccelAngleX = atan2(accY, sqrt(pow(accX,2) + pow(accZ,2)))*RADIANS_TO_DEGREES;
AccelAngleZ = accZ;
#ifdef UNFILTERED
// Compute the (filtered) gyro angles
fAngleX = fgyroX*dt + fLastGyroAngleX;
fAngleY = fgyroY*dt + fLastGyroAngleY;
fAngleZ = fgyroZ*dt + fLastGyroAngleZ;
#endif
#ifdef FILTERED
// Apply the complementary filter to figure out the change in angle - choice of alpha is
// estimated now. Alpha depends on the sampling rate...
const float alpha = 0.9;
float gyroAngleX = fgyroX*dt + fLastGyroAngleX;
float gyroAngleY = fgyroY*dt + fLastGyroAngleY;
float gyroAngleZ = fgyroZ*dt + fLastGyroAngleZ;
fAngleX = alpha*gyroAngleX + (1.0 - alpha)*AccelAngleX;
fAngleY = alpha*gyroAngleY + (1.0 - alpha)*AccelAngleY;
fAngleZ = gyroAngleZ; //Accelerometer doesn't give z-angle
#endif
fLastGyroAngleX = fAngleX; fLastGyroAngleY = fAngleY; fLastGyroAngleZ = fAngleZ;
return true;
}
