Guest User

Untitled

a guest
Feb 23rd, 2012
337
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

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×