SHARE
TWEET

Untitled

a guest Feb 23rd, 2012 285 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.  
  2. #include "math.h"
  3.  
  4. float gPitch = 0;
  5. float gRoll  = 0;
  6. float gYaw   = 0;
  7.  
  8. /* Estimated values by using accelerometer and gyroscope data */
  9. float estimatedX = 0.0;
  10. float estimatedY = 0.0;
  11. float estimatedZ = 0.0;
  12.  
  13. static bool gVeryFirstTime = true;
  14. /* Vector compoments calculated using gyroscope data */
  15. static float gyroscopeX = 0.0;
  16. static float gyroscopeY = 0.0;
  17. static float gyroscopeZ = 0.0;
  18.  
  19. static const float gkWeigth = 5;
  20.  
  21. static void normalize3(float& x, float& y, float& z);
  22.  
  23. void estimateOrientation(float& ax, float& ay, float& az,
  24.                          const float& xz, const float& yz, const float& xy)
  25. {
  26.   /* Calculate vector component from accelerometer data */
  27.   normalize3(ax, ay, az);
  28.  
  29.   if (gVeryFirstTime) {
  30.     estimatedX = ax;
  31.     estimatedY = ay;
  32.     estimatedZ = az;
  33.     gVeryFirstTime = false;
  34.     return;
  35.   }
  36.  
  37.   /* Evaluate vector from gyroscope data */
  38.   if ( -0.1 < estimatedZ && estimatedZ < 0.1) {
  39.     /* The z component is too small, better use previous estimation,
  40.      * otherwise it will simply amplify the error for pitch and roll */
  41.     gyroscopeX = estimatedX;
  42.     gyroscopeY = estimatedY;
  43.     gyroscopeZ = estimatedZ;
  44.   } else {
  45.  
  46.     /* Get angle displacement from gyroscope
  47.      * Datasheet says degree per seconds, but it appears they are
  48.      * in radiants per second
  49.      * Output Data Rate = 100Hz, thus T = 0.01s */
  50.     float deltaPitch = (0.01 * xz) * 180 / M_PI;
  51.     float deltaRoll = (0.01 * yz) * 180 / M_PI;
  52.     float deltaYaw = (0.01 * xy) * 180 / M_PI;
  53.     /* Get angle change in degree */
  54.     gPitch = atan2(estimatedX, estimatedZ) * 180 / M_PI;
  55.     gRoll = atan2(estimatedY, estimatedZ) * 180 / M_PI;
  56.     gYaw = atan2(estimatedX, estimatedY) * 180 / M_PI;
  57.     /* Update angle based on gyroscope readings */
  58.     gPitch += deltaPitch;
  59.     gRoll += deltaRoll;
  60.     gYaw += deltaYaw;
  61.  
  62.     /* Estimate z component sign by looking in what quadrant pitch (xz) is */
  63.     float zSign = (cos(gPitch * M_PI / 180) >= 0) ? 1 : -1;
  64.  
  65.     /* And now the fun part... Reverse calculate R components for gyroscope */
  66.     gyroscopeX = sin(gPitch * M_PI / 180);
  67.     gyroscopeX /= sqrt( 1 + pow(cos(gPitch * M_PI / 180), 2) * pow(tan(gRoll * M_PI / 180), 2));
  68.  
  69.     gyroscopeY = sin(gRoll * M_PI / 180);
  70.     gyroscopeY /= sqrt( 1 + pow(cos(gRoll * M_PI / 180), 2) * pow(tan(gPitch * M_PI / 180), 2));
  71.  
  72.     gyroscopeZ = zSign * sqrt( 1 - gyroscopeX * gyroscopeX - gyroscopeY * gyroscopeY);
  73.   }
  74.  
  75.   /* Combine accelerometer data with gyroscope, hopefully getting better result */
  76.   estimatedX = (ax + gkWeigth * gyroscopeX) / (1 + gkWeigth);
  77.   estimatedY = (ay + gkWeigth * gyroscopeY) / (1 + gkWeigth);
  78.   estimatedZ = (az + gkWeigth * gyroscopeZ) / (1 + gkWeigth);
  79.  
  80.   /* Normalize vector */
  81.   normalize3(estimatedX, estimatedY, estimatedZ);
  82.  
  83.   return ;
  84. }
  85.  
  86. void normalize3(float& x, float& y, float& z)
  87. {
  88.   float r = sqrt(x*x + y*y + z*z);
  89.   x /= r;
  90.   y /= r;
  91.   z /= r;
  92. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top