Hello,
First I wish anyone could help me here, this is my first post on arduino.
I bought a GY-87 10-DOF-IMU Chip that I'm willing to use for my quad-copter project later after I first know how to make it work.
After searching and trying between many codes, I could make the BMP work or at least it's getting a believable reads.
The MPU also is working fine except that the code doesn't showing the Z axis which I need and can't really understand how to get it, it's using KALMAN filter btw.
finally the real problem, the HMC, GY-87 comes with a HMC5883L, which was stuck at 50.3 degrees, after modifications and combining an I2C code, it could work but when I rotate it the value changes only from 200 to 350 or so, it's not working fine ...
How to make the compass (HMC5883L) work functionally
How to get the Z axis of the MPU6050
wish anyone could answer me cuz I'm really disappointed with that sensor 
The main code :-
#include "Kalman.h"
#include <Wire.h>
#include <HMC5883L.h>
#include <Adafruit_BMP085.h>
Adafruit_BMP085 bmp;
HMC5883L compass;
int error = 0;
int ac1 ,ac2 ,ac3;
int b1,b2,mb,mc,md,b5;
const unsigned char OSS = 0;
unsigned int ac4 ,ac5 ,ac6;
Kalman kalmanX; Kalman kalmanY;
double accX, accY, accZ,gyroX, gyroY, gyroZ;
double gyroXangle, gyroYangle, compAngleX, compAngleY, kalAngleX, kalAngleY;
int16_t tempRaw;
uint32_t timer;
uint8_t i2cData[14];
void setup() {
Serial.begin(115200);
Wire.begin();
TWBR = ((F_CPU / 400000L) - 16) / 2;
i2cData[0] = 7;
i2cData[1] = 0x00;
i2cData[2] = 0x00;
i2cData[3] = 0x00;
while (i2cWrite(0x19, i2cData, 4, false));
while (i2cWrite(0x6B, 0x01, true));
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) {
Serial.print(F("Error reading sensor"));
while (1);
Serial.println("Starting the I2C interface.");
Serial.println("Constructing new HMC5883L");
compass = HMC5883L();
Serial.println("Setting scale to +/- 1.3 Ga");
error = compass.SetScale(1.3);
if(error != 0)
Serial.println(compass.GetErrorText(error));
Serial.println("Setting measurement mode to continous.");
error = compass.SetMeasurementMode(Measurement_Continuous);
if(error != 0)
Serial.println(compass.GetErrorText(error));
}
delay(100);
if (!bmp.begin()) {
Serial.println("Could not find a valid BMP085 sensor, check wiring!");
while (1) {}
}
delay(100);
while (i2cRead(0x3B, i2cData, 6));
accX = (i2cData[0] << 8) | i2cData[1];
accY = (i2cData[2] << 8) | i2cData[3];
accZ = (i2cData[4] << 8) | i2cData[5];
#ifdef RESTRICT_PITCH
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
timer = micros();
}
void loop() {
//****************** BMP085 *****************\\
float temperature = bmp.readTemperature();
float pressure = bmp.readPressure();
float atm = bmp.readSealevelPressure();
float altitude = bmp.readAltitude();
Serial.print("Standard Atmosphere: ");
Serial.println(pressure / 101325, 4); //display 4 decimal places
Serial.print("Pressure at sealevel (calculated) = ");
Serial.print(atm, 4);
Serial.println(" Pa");
Serial.print("Temperature: ");
Serial.print(temperature, 2); //display 2 decimal places
Serial.println("deg C");
Serial.print("Altitude: ");
Serial.print(altitude, 2); //display 2 decimal places
Serial.println(" M");
Serial.println();//line break
delay(500); //wait a second and get values again.
//******************** MBU6050 ************************\\
while (i2cRead(0x3B, i2cData, 14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = (i2cData[6] << 8) | i2cData[7];
gyroX = (i2cData[8] << 8) | i2cData[9];
gyroY = (i2cData[10] << 8) | i2cData[11];
gyroZ = (i2cData[12] << 8) | i2cData[13];
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
#ifdef RESTRICT_PITCH
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
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;
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;
#if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
#endif
#if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
#endif
Serial.print("Roll (X)");Serial.print(kalAngleX); Serial.print("\t");
Serial.print("Pitch (Y)"); Serial.print(kalAngleY); Serial.print("\t");
delay(2);
//********************** compass ***************************\\
MagnetometerRaw raw = compass.ReadRawAxis();
MagnetometerScaled scaled = compass.ReadScaledAxis();
int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)
float heading = atan2(scaled.YAxis, scaled.XAxis);
float declinationAngle = 0.0215;
heading += declinationAngle;
if(heading < 0)
heading += 2*PI;
if(heading > 2*PI)
heading -= 2*PI;
float headingDegrees = heading * 180/M_PI;
Output(headingDegrees);
}
void Output(float headingDegrees)
{
Serial.print("Compass Heading:\t");
Serial.print(headingDegrees);
Serial.println(" Degrees \t");
delay(100);
}
there is another tab for KALMAN.h and I2C, u can find them here :-
Project Files
Project Libs
