Grumpy_Mike:
See that commented out print statement in the readQ function. Un comment it an see if anything is being recieved from your arduino.
You. Eed to post your arduino code as well.
here is arduino code
#include <Wire.h>
#include "Kalman.h"
#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead - please read: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
Kalman kalmanX, kalmanY, kalmanZ; // Create the Kalman instances
const uint8_t MPU6050 = 0x68; // If AD0 is logic low on the PCB the address is 0x68, otherwise set this to 0x69
const uint8_t HMC5883L = 0x1E; // Address of magnetometer
/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
double magX, magY, magZ;
int16_t tempRaw;
double roll, pitch, yaw; // Roll and pitch are calculated using the accelerometer while yaw is calculated using the magnetometer
double gyroXangle, gyroYangle, gyroZangle; // Angle calculate using the gyro only
double compAngleX, compAngleY, compAngleZ; // Calculated angle using a complementary filter
double kalAngleX, kalAngleY, kalAngleZ; // Calculated angle using a Kalman filter
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
// TODO: Lav zero rutine til accelerometer, magnetomer og gyroscope
#define MAG0MAX 603
#define MAG0MIN -578
#define MAG1MAX 542
#define MAG1MIN -701
#define MAG2MAX 547
#define MAG2MIN -556
float magOffset[3] = { (MAG0MAX + MAG0MIN) / 2, (MAG1MAX + MAG1MIN) / 2, (MAG2MAX + MAG2MIN) / 2 };
double magGain[3];
void setup() {
delay(100); // Wait for sensors to get ready
Serial.begin(115200);
Wire.begin();
TWBR = ((F_CPU / 400000L) - 16) / 2; // Set I2C frequency to 400kHz
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while (i2cWrite(MPU6050, 0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(MPU6050, 0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(MPU6050, 0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
while (i2cWrite(HMC5883L, 0x02, 0x00, true)); // Configure device for continuous mode
// TODO: Sæt sample rate
delay(100); // Wait for sensors to stabilize
/* Set Kalman and gyro starting angle */
updateMPU6050();
updateHMC5883L();
updatePitchRoll();
updateYaw();
kalmanX.setAngle(roll); // First set roll starting angle
gyroXangle = roll;
compAngleX = roll;
kalmanY.setAngle(pitch); // Then pitch
gyroYangle = pitch;
compAngleY = pitch;
kalmanZ.setAngle(yaw); // And finally yaw
gyroZangle = yaw;
compAngleZ = yaw;
timer = micros(); // Initialize the timer
}
void loop() {
/* Update all the IMU values */
updateMPU6050();
updateHMC5883L();
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
/* Roll and pitch estimation */
updatePitchRoll();
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 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 restricted 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 restricted accelerometer reading
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
/* Yaw estimation */
updateYaw();
double gyroZrate = gyroZ / 131.0; // Convert to deg/s
// This fixes the transition problem when the yaw angle jumps between -180 and 180 degrees
if ((yaw < -90 && kalAngleZ > 90) || (yaw > 90 && kalAngleZ < -90)) {
kalmanZ.setAngle(yaw);
compAngleZ = yaw;
kalAngleZ = yaw;
gyroZangle = yaw;
} else
kalAngleZ = kalmanZ.getAngle(yaw, gyroZrate, dt); // Calculate the angle using a Kalman filter
/* Estimate angles using gyro only */
gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle += gyroYrate * dt;
gyroZangle += gyroZrate * dt;
//gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate from the Kalman filter
//gyroYangle += kalmanY.getRate() * dt;
//gyroZangle += kalmanZ.getRate() * dt;
/* Estimate angles using complimentary filter */
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;
compAngleZ = 0.93 * (compAngleZ + gyroZrate * dt) + 0.07 * yaw;
// Reset the gyro angles when they has drifted too much
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
if (gyroZangle < -180 || gyroZangle > 180)
gyroZangle = kalAngleZ;
Serial.print(kalAngleX); Serial.print("\t");
Serial.print(kalAngleY); Serial.print("\t");
Serial.print(kalAngleZ); Serial.print("\t");
delay(10);
}
void updateMPU6050() {
while (i2cRead(MPU6050, 0x3B, i2cData, 14)); // Get accelerometer and gyroscope values
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];
}
void updateHMC5883L() {
while (i2cRead(HMC5883L, 0x03, i2cData, 6)); // Get magnetometer values
magX = ((i2cData[0] << 8) | i2cData[1]);
magZ = ((i2cData[2] << 8) | i2cData[3]);
magY = ((i2cData[4] << 8) | i2cData[5]);
}
void updatePitchRoll() {
#ifdef RESTRICT_PITCH // Eq. 25 and 26
roll = atan2(accY, accZ) * RAD_TO_DEG;
pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
}
void updateYaw() { /
magX *= -1; // Invert axis - this it done here, as it should be done after the calibration
magZ *= -1;
magX *= magGain[0];
magY *= magGain[1];
magZ *= magGain[2];
magX -= magOffset[0];
magY -= magOffset[1];
magZ -= magOffset[2];
double rollAngle = kalAngleX * DEG_TO_RAD;
double pitchAngle = kalAngleY * DEG_TO_RAD;
double Bfy = magZ * sin(rollAngle) - magY * cos(rollAngle);
double Bfx = magX * cos(pitchAngle) + magY * sin(pitchAngle) * sin(rollAngle) + magZ * sin(pitchAngle) * cos(rollAngle);
yaw = atan2(-Bfy, Bfx) * RAD_TO_DEG;
yaw *= -1;
}