Hi,
I'm in the midst of a project where i need to use the gyroscope/accelerometer sensor MPU-6050. The project attempts to estimate the angle of the sensor using both the accelerometer and the gyroscope readings. The first characterised by stability over time and the second by short time stability with long time drift. I've therefore decided to try implement a complementary filter. I'm using Arduino nano.
The problem I'm facing is that the complementary filter spits out values that seemingly show only accelerometer characteristics OR only gyroscope characteristics. I'm now fine tuning at the 11th decimal of the filter coefficient, which seems unreasonable. I judge the characteristics by watching the serial plotter while moving the sensor quickly from side to side, something that only affects the accelerometer angle estimation method.
I've tried:
-
At great length fine tuning the filter coefficient.
-
increasing diffTime (dt) using an if statement.
-
Relocating the time logic inside the loop.
Is there something I am missing? * I should also note that while I know there are complementary filter libraries, I would really wish to pursue this method, as it is something I want to know how to implement. I also excuse any language mistakes or failures in following post guidelines.
#include <RH_ASK.h>
#include <SPI.h> // Not actually used but needed to compile
#include <Wire.h>
RH_ASK driver(2000, 11, 4, 12);
uint8_t angleToSend;
uint8_t valueLength;
// Variables for gyroscope
const int Mpu = 0x68; //I2C adress for coms
float accX, accY, accZ, angVelocityZ;
float angleApprox1, angleApprox2, currentAngle, tempAngle;
// Filter constant for complimentary filter, at value 0.99.....702 the filter potray only gyro characteristics!
// There seem to be a very distinct switch going from accelerometer to gyro behaviour.
const int filterConstant = 0.99999997015;
// Time variables
float currentTime, previousTime, diffTime;
void setup() {
Serial.begin(19200);
Serial.println("Serial OK");
Serial.println("");
Wire.begin();
Serial.println("Wire OK");
if (!driver.init())
Serial.println("init failed");
else {
Serial.println("RH driver OK");
}
// Perform MPU signal reset
Wire.beginTransmission(Mpu);
Wire.write(0x6B); // Talk to register 6B
Wire.write(0x00); // Reset - leaves calibration intact
Wire.endTransmission(true);
// Set the correct acceleration scale
Wire.beginTransmission(Mpu);
Wire.write(0x1C); // Talk to register 1C
Wire.write(0b00000000); // Set scale to 2g, perform no self test
Wire.endTransmission(true);
// Make singular measurement to provide staring point for angular velocity approximation method
Wire.beginTransmission(Mpu);
Wire.write(0x3B); // Start at register 3B
Wire.endTransmission(false); // Keep the I2C bus open
Wire.requestFrom(Mpu, 6, true);
accX = (Wire.read() << 8 | Wire.read()) / 16384.0;
accY = (Wire.read() << 8 | Wire.read()) / 16384.0;
accZ = (Wire.read() << 8 | Wire.read()) / 16384.0 - 0.1;
// Calculate angle of sensor
currentAngle = atan2(accY, -1 * accX) * 180 / 3.1416;
currentTime = micros();
}
void loop() {
// Get acceleration readings
Wire.beginTransmission(Mpu);
Wire.write(0x3B); // Start at register 3B
Wire.endTransmission(false); // Keep the I2C bus open
Wire.requestFrom(Mpu, 6, true);
// Read 8 bits and store in 16 bit integer, shift to the left. The or operation overlays the next reading.
accX = (Wire.read() << 8 | Wire.read()) / 16384.0; // Division according to datasheet page 29, unit is [G].
accY = (Wire.read() << 8 | Wire.read()) / 16384.0;
accZ = (Wire.read() << 8 | Wire.read()) / 16384.0 - 0.1;
// Get angular velocity around x-axis
Wire.beginTransmission(Mpu);
Wire.write(0x47); // Start at register 43 (Hex)
Wire.endTransmission(false); // Keep the I2C bus open
Wire.requestFrom(Mpu, 2, true);
angVelocityZ = (Wire.read() << 8 | Wire.read()) / 131 + 5; // Division according to datasheet page 31
// Update time variables
previousTime = currentTime;
currentTime = micros();
diffTime = (currentTime - previousTime) * 0.000001;
// Use complementary filter
angleApprox1 = currentAngle + angVelocityZ * diffTime;
angleApprox2 = atan2(accY, -1 * accX) * 180 / 3.1416;
currentAngle = filterConstant * angleApprox1 + (1 - filterConstant) * angleApprox2;
tempAngle = currentAngle;
// Constrain angle to 0 < angle 180
if (tempAngle < 0 && tempAngle > -90) {
tempAngle = 0;
} else if (tempAngle >= -180 && tempAngle < -90) {
tempAngle = 180;
}
angleToSend = int(tempAngle);
// Send angle
valueLength = sizeof(angleToSend);
driver.send(&angleToSend, valueLength);
driver.waitPacketSent();
Serial.print(angleToSend);
Serial.print(" ");
Serial.print(angleApprox1);
Serial.print(" ");
Serial.print(angleApprox2);
Serial.print(" ");
Serial.println(diffTime);
}
