Hello,
For my project i'm working with an MPU-6050 Accelerometer. My goal is to get an accurate and steady angle out of the accelerometer when it is not experiencing any major movements, but it needs to be quick to handle sudden changes in angle.
The problem:
To smooth out the data recieved I used the default smoothing example. This works great, but if I smooth my data with 100 samples, it takes too long to reach the new value of the angle when a big angle shift occurs. (so for example: if I move from 90 to 0 degrees, it will take too long to get to 0: the raw sensor data is already at 0, but the smoothed data is delayed by the smoothing)
I thought of a solution to bypass the smoothing while the value change is high. so:
float sensorData() {
// calculate the rate of change from the sensor
if (millis() - lastTime >= dt)
{
lastTime = millis();
angle = pitch * (360 / 1023.0);
rate = (angle-LastAngle);
LastAngle = angle;
}
changeRate = (1000*rate/dt); // rate of change
if (changeRate < 0.03 && changeRate > -0.03) { // if the changerate is low (sensor = steady)
// Smoothen the data
total = total - readings[readIndex];
readings[readIndex] = pitch;
total = total + readings[readIndex];
readIndex = readIndex + 1;
if (readIndex >= numReadings) {
readIndex = 0;
}
average = total / numReadings; // average = smoothed data
} else { // // if the changerate is high (sensor = moving)
average = pitch; // skip smoothing and use the pitchdata directly
total = pitch * 100; // Create a rough total value for the sensor smoothing to continue on when sensor is steady again
}
// Debug
Serial.print("pitch: ");
Serial.print(pitch);
Serial.print(" readings[readIndex]: ");
Serial.print(readings[readIndex]);
Serial.print(" average: ");
Serial.println(average);
return average;
delay (1);
}
This is however not working. I think this is due to the fact that the smoothing array (readings[readIndex]) is still filled with values from the time the sensor was steady. So when the sensor is steady again this old array is substracted from the new rough total (total = pitch * 100;) that is generated while the sensor is moving. This results in a difference between sensor value and average value. This difference persists as it keeps calculating with a now false total.
I'm very new to this, so i'm not sure where to go from here. Am I approaching this all wrong? Any input will be greatly appreciated!