Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #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;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement