Thank you for that document, I will read through it thoroughly. If accelerometer measurements are skewed by vertical acceleration does that mean my gyroscope measurements will be off? I have put my code below (w/o the apogee part). PS i did not write the entire code myself.
/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
// Source: https://github.com/TKJElectronics/KalmanFilter
Kristian Lauszus, TKJ Electronics
Web : http://www.tkjelectronics.com
e-mail : kristianl@tkjelectronics.com
*/
#include <SPI.h>
#include <SD.h>
#include <Servo.h>
#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; // Create the Kalman instances
Kalman kalmanY;
/* IMU Data */
double accX, accY, accZ;
double gyroX, gyroY, gyroZ;
int16_t tempRaw;
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
uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data
const float CorrectInt = -1.5;
const int Ser1up = 88;
const int Ser2up = 77;
const int Ser3up = 82;
const int Ser4up = 88;
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
int LEDred = 13;
int LEDwht = 6;
const int chipSelect = 10;
// TODO: Make calibration routine
File ANTAREZ;
void setup() {
pinMode(LEDred, OUTPUT);
pinMode(LEDwht, OUTPUT);
digitalWrite(LEDred, HIGH);
delay(200);
digitalWrite(LEDred, LOW);
delay(100);
digitalWrite(LEDred, HIGH);
delay(200);
digitalWrite(LEDred, LOW);
delay(100);
digitalWrite(LEDred, HIGH);
delay(200);
digitalWrite(LEDred, LOW);
servo1.attach(2);
servo2.attach(3);
servo3.attach(4);
servo4.attach(5);
Serial.begin(115200);
Wire.begin();
#if ARDUINO >= 157
Wire.setClock(400000UL); // Set I2C frequency to 400kHz
#else
TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif
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(0x19, i2cData, 4, false)); // Write to all four registers at once
while (i2cWrite(0x6B, 0x01, true)); // PLL with X axis gyroscope reference and disable sleep mode
while (i2cRead(0x75, i2cData, 1));
if (i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while (1);
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while (i2cRead(0x3B, i2cData, 6));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
// It is then converted from radians to degrees
#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
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
Serial.print("Initializing SD card...");
if(!SD.begin(chipSelect))
{
digitalWrite(LEDred, HIGH);
delay(200);
digitalWrite(LEDred, LOW);
delay(100);
digitalWrite(LEDred, HIGH);
delay(200);
digitalWrite(LEDred, LOW);
delay(100);
digitalWrite(LEDred, HIGH);
delay(200);
digitalWrite(LEDred, LOW);
}
servo1.write(45);
servo2.write(135);
servo3.write(135);
servo4.write(45);
delay(2000);
servo1.write(135);
servo2.write(45);
servo3.write(45);
servo4.write(135);
delay(2000);
servo1.write(Ser1up);
servo2.write(Ser2up);
servo3.write(Ser3up);
servo4.write(Ser4up);
delay(2000);
timer = micros();
}
void loop() {
/* Update all the values */
digitalWrite(LEDwht, HIGH);
while (i2cRead(0x3B, i2cData, 14));
accX = (int16_t)((i2cData[0] << 8) | i2cData[1]);
accY = (int16_t)((i2cData[2] << 8) | i2cData[3]);
accZ = (int16_t)((i2cData[4] << 8) | i2cData[5]);
tempRaw = (int16_t)((i2cData[6] << 8) | i2cData[7]);
gyroX = (int16_t)((i2cData[8] << 8) | i2cData[9]);
gyroY = (int16_t)((i2cData[10] << 8) | i2cData[11]);
gyroZ = (int16_t)((i2cData[12] << 8) | i2cData[13]);;
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
// It is then converted from radians to degrees
#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
// 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;
/* Print Data */
#if 0 // Set to 1 to activate
Serial.print(accX); Serial.print("\t");
Serial.print(accY); Serial.print("\t");
Serial.print(accZ); Serial.print("\t");
Serial.print(gyroX); Serial.print("\t");
Serial.print(gyroY); Serial.print("\t");
Serial.print(gyroZ); Serial.print("\t");
Serial.print("\t");
#endif
Serial.print(kalAngleX); Serial.print("\t");
Serial.println(kalAngleY); Serial.print("\t");
servo1.write((kalAngleY * CorrectInt) + Ser1up);
servo2.write(((kalAngleY * CorrectInt) - Ser2up) * -1);
servo3.write((kalAngleX * CorrectInt) + Ser3up);
servo4.write(((kalAngleX * CorrectInt) - Ser4up) * -1);
//int Time = micros();
ANTAREZ = SD.open("datalog.txt", FILE_WRITE);
// if the file is available, write to it:
if (ANTAREZ) {
//ANTAREZ.print(Time);
ANTAREZ.print(", ");
ANTAREZ.print(kalAngleY);
ANTAREZ.print(", ");
ANTAREZ.println(kalAngleX);
ANTAREZ.close();
}
else {
Serial.println("error opening datalog.txt");
}
delay(2);
}