changeset 148:41c1aff71207 main

More updates to the averaging method.
author Bob Cook <bob@bobcookdev.com>
date Sun, 12 Aug 2012 21:25:46 -0700
parents 73f4dc874231
children ebede8ccd1f4
files main/robots/odr/ImuReader.cpp
diffstat 1 files changed, 100 insertions(+), 36 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr/ImuReader.cpp	Sat Aug 11 21:40:53 2012 -0700
+++ b/main/robots/odr/ImuReader.cpp	Sun Aug 12 21:25:46 2012 -0700
@@ -338,33 +338,90 @@
 }
 
 // ----------------------------------------------------------------------------------------
+//  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.
+//
+//  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.
 
-static double computeYawAverage( const std::vector< double >& samples )
+static double computeAverage( const std::vector< double >& samples, double maxValue )
 {
-    double deltaValuesSum = 0.0;
+    bool valuesAreNearMax = false;
+    bool valuesAreNearNegMax = false;
 
     for ( std::vector< double >::const_iterator itr = samples.begin();
           itr != samples.end();
           ++itr )
     {
-        if ( *itr < 0.0 )
+        if ( *itr > ( maxValue - 10.0 ) ) valuesAreNearMax = true;
+        if ( *itr < ( maxValue + 10.0 ) ) valuesAreNearNegMax = true;
+    }
+
+    if ( valuesAreNearMax && valuesAreNearNegMax )
+    {
+        // wrap-around case computes the delta from 180/-180
+
+        double deltaValuesSum = 0.0;
+
+        for ( std::vector< double >::const_iterator itr = samples.begin();
+              itr != samples.end();
+              ++itr )
         {
-            deltaValuesSum += ( -180.0 - *itr );
+            if ( *itr < 0.0 )
+            {
+                deltaValuesSum += ( -maxValue - *itr );
+            }
+            else
+            {
+                deltaValuesSum += ( maxValue - *itr );
+            }
+        }
+
+        if ( deltaValuesSum < 0.0 )
+        {
+            return -maxValue - ( deltaValuesSum / samples.size() );
         }
         else
         {
-            deltaValuesSum += ( 180.0 - *itr );
+            return maxValue - ( deltaValuesSum / samples.size() );
         }
     }
 
-    if ( deltaValuesSum < 0.0 )
+    // just compute a regular average
+
+    double valuesSum = 0.0;
+
+    for ( std::vector< double >::const_iterator itr = samples.begin();
+          itr != samples.end();
+          ++itr )
     {
-        return -180.0 - ( deltaValuesSum / samples.size() );
+        valuesSum += *itr;
     }
-    else
-    {
-        return 180.0 - ( deltaValuesSum / samples.size() );
-    }
+
+    return valuesSum / samples.size();
+}
+
+// ----------------------------------------------------------------------------------------
+
+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 );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -387,10 +444,18 @@
 
             resetParseState();
 
-            std::vector< double > yawSamples;
-            yawSamples.reserve( 30 );
+            // keep the last 10 values to average together (approx 0.5 seconds elapsed)
+
+            std::vector< double > valuesRoll;
+            valuesRoll.reserve( 10 );
 
-            int valuesAvailableCount = 0;
+            std::vector< double > valuesPitch;
+            valuesPitch.reserve( 10 );
+
+            std::vector< double > valuesYaw;
+            valuesYaw.reserve( 10 );
+
+            // loop while we are still getting data
 
             for ( ;; )
             {
@@ -405,39 +470,38 @@
 
                 if ( ! readFromSerialPort( buffer, &length ) )
                 {
-                    break; // error
+                    break; // error, drop out and reinit the port
                 }
 
                 if ( processImuData( buffer, length ) )
                 {
-                    // new values available
+                    // new values available, save them
 
-                    yawSamples.push_back( m_parseValueYaw );
+                    valuesRoll.push_back( m_parseValueRoll );
+                    valuesPitch.push_back( m_parseValuePitch );
+                    valuesYaw.push_back( m_parseValueYaw );
 
-                    if ( yawSamples.size() == 30 )
+                    // 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 )
                     {
-                        double  yawAverage = computeYawAverage( yawSamples );
-                        yawSamples.erase( yawSamples.begin(), yawSamples.begin() + 10 );
-
-                        double delta = yawAverage - m_parseValueYaw;
+                        double  averageRoll  = computeAverageRoll( valuesRoll );
+                        double  averagePitch = computeAveragePitch( valuesPitch );
+                        double  averageYaw   = computeAverageYaw( valuesYaw );
 
                         log.information(
                             Poco::Logger::format(
-                                "ImuReader average yaw: $0 (delta: $1)",
-                                Poco::NumberFormatter::format( yawAverage ),
-                                Poco::NumberFormatter::format( delta ) ) );
-                    }
+                                "ImuReader average roll: $0 / pitch: $1 / yaw: $2",
+                                Poco::NumberFormatter::format( averageRoll ),
+                                Poco::NumberFormatter::format( averagePitch ),
+                                Poco::NumberFormatter::format( averageYaw ) ) );
 
-                    // new values available
-                    if ( ++valuesAvailableCount == 10 )
-                    {
-                        valuesAvailableCount = 0;
-                        log.information(
-                            Poco::Logger::format(
-                                "ImuReader found roll: $0 pitch: $1 yaw: $2",
-                                Poco::NumberFormatter::format( m_parseValueRoll ),
-                                Poco::NumberFormatter::format( m_parseValuePitch ),
-                                Poco::NumberFormatter::format( m_parseValueYaw ) ) );
+                        // clear the vectors, to accumulate more samples
+
+                        valuesRoll.clear();
+                        valuesPitch.clear();
+                        valuesYaw.clear();
                     }
                 }
             }