changeset 149:ebede8ccd1f4 main

All we really need is the average Yaw. Everything else is ok for instantaneous values.
author Bob Cook <bob@bobcookdev.com>
date Mon, 13 Aug 2012 19:22:43 -0700
parents 41c1aff71207
children cbbd86558aa0
files main/robots/odr/ImuReader.cpp
diffstat 1 files changed, 16 insertions(+), 51 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr/ImuReader.cpp	Sun Aug 12 21:25:46 2012 -0700
+++ b/main/robots/odr/ImuReader.cpp	Mon Aug 13 19:22:43 2012 -0700
@@ -338,15 +338,13 @@
 }
 
 // ----------------------------------------------------------------------------------------
-//  The values range from 0 to maxvalue then from -maxvalue to 0 again. This means the
-//  values near zero are very close together (as the sensor turns) but so are values at
-//  the maximum/-maximum.
+//  Yaw values range from 0 to 180 then from -180 to 0 again. This means the values near
+//  zero are very close together (as the sensor turns) but so are values at 180/-180.
 //
-//  For values that aren't approaching the maximum, a standard average is ok. But for the
-//  values on either side of the maximum/-maximum boundry we compute the average delta
-//  from the maximum/-maximum value.
+//  For values that aren't approaching 180, a standard average is ok. But for the values
+//  on either side of the 180/-180 boundry we compute the average delta from 180/-180.
 
-static double computeAverage( const std::vector< double >& samples, double maxValue )
+static double computeAverageYaw( const std::vector< double >& samples )
 {
     bool valuesAreNearMax = false;
     bool valuesAreNearNegMax = false;
@@ -355,8 +353,8 @@
           itr != samples.end();
           ++itr )
     {
-        if ( *itr > ( maxValue - 10.0 ) ) valuesAreNearMax = true;
-        if ( *itr < ( maxValue + 10.0 ) ) valuesAreNearNegMax = true;
+        if ( *itr > (  180.0 - 10.0 ) ) valuesAreNearMax = true;
+        if ( *itr < ( -180.0 + 10.0 ) ) valuesAreNearNegMax = true;
     }
 
     if ( valuesAreNearMax && valuesAreNearNegMax )
@@ -371,21 +369,21 @@
         {
             if ( *itr < 0.0 )
             {
-                deltaValuesSum += ( -maxValue - *itr );
+                deltaValuesSum += ( -180.0 - *itr );
             }
             else
             {
-                deltaValuesSum += ( maxValue - *itr );
+                deltaValuesSum += ( 180.0 - *itr );
             }
         }
 
         if ( deltaValuesSum < 0.0 )
         {
-            return -maxValue - ( deltaValuesSum / samples.size() );
+            return -180.0 - ( deltaValuesSum / samples.size() );
         }
         else
         {
-            return maxValue - ( deltaValuesSum / samples.size() );
+            return 180.0 - ( deltaValuesSum / samples.size() );
         }
     }
 
@@ -405,27 +403,6 @@
 
 // ----------------------------------------------------------------------------------------
 
-static double computeAverageRoll( const std::vector< double >& samples )
-{
-    return computeAverage( samples, 180.0 );
-}
-
-// ----------------------------------------------------------------------------------------
-
-static double computeAveragePitch( const std::vector< double >& samples )
-{
-    return computeAverage( samples, 90.0 );
-}
-
-// ----------------------------------------------------------------------------------------
-
-static double computeAverageYaw( const std::vector< double >& samples )
-{
-    return computeAverage( samples, 180.0 );
-}
-
-// ----------------------------------------------------------------------------------------
-
 void ImuReader::run()
 {
     Poco::Logger& log = Poco::Logger::get( m_loggerName );
@@ -446,12 +423,6 @@
 
             // keep the last 10 values to average together (approx 0.5 seconds elapsed)
 
-            std::vector< double > valuesRoll;
-            valuesRoll.reserve( 10 );
-
-            std::vector< double > valuesPitch;
-            valuesPitch.reserve( 10 );
-
             std::vector< double > valuesYaw;
             valuesYaw.reserve( 10 );
 
@@ -477,30 +448,24 @@
                 {
                     // new values available, save them
 
-                    valuesRoll.push_back( m_parseValueRoll );
-                    valuesPitch.push_back( m_parseValuePitch );
                     valuesYaw.push_back( m_parseValueYaw );
 
                     // if we've reached 10 values, compute the average and update the
                     // scoreboard; then clear the vector making room for new values
 
-                    if ( valuesRoll.size() == 10 )
+                    if ( valuesYaw.size() == 10 )
                     {
-                        double  averageRoll  = computeAverageRoll( valuesRoll );
-                        double  averagePitch = computeAveragePitch( valuesPitch );
-                        double  averageYaw   = computeAverageYaw( valuesYaw );
+                        double averageYaw = computeAverageYaw( valuesYaw );
 
                         log.information(
                             Poco::Logger::format(
                                 "ImuReader average roll: $0 / pitch: $1 / yaw: $2",
-                                Poco::NumberFormatter::format( averageRoll ),
-                                Poco::NumberFormatter::format( averagePitch ),
+                                Poco::NumberFormatter::format( m_parseValueRoll ),
+                                Poco::NumberFormatter::format( m_parseValuePitch ),
                                 Poco::NumberFormatter::format( averageYaw ) ) );
 
-                        // clear the vectors, to accumulate more samples
+                        // clear the vector, to accumulate more samples
 
-                        valuesRoll.clear();
-                        valuesPitch.clear();
                         valuesYaw.clear();
                     }
                 }