#include "math.h"
float gPitch = 0;
float gRoll = 0;
float gYaw = 0;
/* Estimated values by using accelerometer and gyroscope data */
float estimatedX = 0.0;
float estimatedY = 0.0;
float estimatedZ = 0.0;
static bool gVeryFirstTime = true;
/* Vector compoments calculated using gyroscope data */
static float gyroscopeX = 0.0;
static float gyroscopeY = 0.0;
static float gyroscopeZ = 0.0;
static const float gkWeigth = 5;
static void normalize3(float& x, float& y, float& z);
void estimateOrientation(float& ax, float& ay, float& az,
const float& xz, const float& yz, const float& xy)
{
/* Calculate vector component from accelerometer data */
normalize3(ax, ay, az);
if (gVeryFirstTime) {
estimatedX = ax;
estimatedY = ay;
estimatedZ = az;
gVeryFirstTime = false;
return;
}
/* Evaluate vector from gyroscope data */
if ( -0.1 < estimatedZ && estimatedZ < 0.1) {
/* The z component is too small, better use previous estimation,
* otherwise it will simply amplify the error for pitch and roll */
gyroscopeX = estimatedX;
gyroscopeY = estimatedY;
gyroscopeZ = estimatedZ;
} else {
/* Get angle displacement from gyroscope
* Datasheet says degree per seconds, but it appears they are
* in radiants per second
* Output Data Rate = 100Hz, thus T = 0.01s */
float deltaPitch = (0.01 * xz) * 180 / M_PI;
float deltaRoll = (0.01 * yz) * 180 / M_PI;
float deltaYaw = (0.01 * xy) * 180 / M_PI;
/* Get angle change in degree */
gPitch = atan2(estimatedX, estimatedZ) * 180 / M_PI;
gRoll = atan2(estimatedY, estimatedZ) * 180 / M_PI;
gYaw = atan2(estimatedX, estimatedY) * 180 / M_PI;
/* Update angle based on gyroscope readings */
gPitch += deltaPitch;
gRoll += deltaRoll;
gYaw += deltaYaw;
/* Estimate z component sign by looking in what quadrant pitch (xz) is */
float zSign = (cos(gPitch * M_PI / 180) >= 0) ? 1 : -1;
/* And now the fun part... Reverse calculate R components for gyroscope */
gyroscopeX = sin(gPitch * M_PI / 180);
gyroscopeX /= sqrt( 1 + pow(cos(gPitch * M_PI / 180), 2) * pow(tan(gRoll * M_PI / 180), 2));
gyroscopeY = sin(gRoll * M_PI / 180);
gyroscopeY /= sqrt( 1 + pow(cos(gRoll * M_PI / 180), 2) * pow(tan(gPitch * M_PI / 180), 2));
gyroscopeZ = zSign * sqrt( 1 - gyroscopeX * gyroscopeX - gyroscopeY * gyroscopeY);
}
/* Combine accelerometer data with gyroscope, hopefully getting better result */
estimatedX = (ax + gkWeigth * gyroscopeX) / (1 + gkWeigth);
estimatedY = (ay + gkWeigth * gyroscopeY) / (1 + gkWeigth);
estimatedZ = (az + gkWeigth * gyroscopeZ) / (1 + gkWeigth);
/* Normalize vector */
normalize3(estimatedX, estimatedY, estimatedZ);
return ;
}
void normalize3(float& x, float& y, float& z)
{
float r = sqrt(x*x + y*y + z*z);
x /= r;
y /= r;
z /= r;
}