changeset 147:73f4dc874231 main

Further improvements to the IMU parser, experimenting with averageing the yaw values.
author Bob Cook <bob@bobcookdev.com>
date Sat, 11 Aug 2012 21:40:53 -0700
parents a01a4a349d58
children 41c1aff71207
files main/robots/odr/ImuReader.cpp
diffstat 1 files changed, 51 insertions(+), 0 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr/ImuReader.cpp	Sat Aug 11 16:06:03 2012 -0700
+++ b/main/robots/odr/ImuReader.cpp	Sat Aug 11 21:40:53 2012 -0700
@@ -339,6 +339,36 @@
 
 // ----------------------------------------------------------------------------------------
 
+static double computeYawAverage( const std::vector< double >& samples )
+{
+    double deltaValuesSum = 0.0;
+
+    for ( std::vector< double >::const_iterator itr = samples.begin();
+          itr != samples.end();
+          ++itr )
+    {
+        if ( *itr < 0.0 )
+        {
+            deltaValuesSum += ( -180.0 - *itr );
+        }
+        else
+        {
+            deltaValuesSum += ( 180.0 - *itr );
+        }
+    }
+
+    if ( deltaValuesSum < 0.0 )
+    {
+        return -180.0 - ( deltaValuesSum / samples.size() );
+    }
+    else
+    {
+        return 180.0 - ( deltaValuesSum / samples.size() );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
 void ImuReader::run()
 {
     Poco::Logger& log = Poco::Logger::get( m_loggerName );
@@ -357,6 +387,9 @@
 
             resetParseState();
 
+            std::vector< double > yawSamples;
+            yawSamples.reserve( 30 );
+
             int valuesAvailableCount = 0;
 
             for ( ;; )
@@ -378,6 +411,24 @@
                 if ( processImuData( buffer, length ) )
                 {
                     // new values available
+
+                    yawSamples.push_back( m_parseValueYaw );
+
+                    if ( yawSamples.size() == 30 )
+                    {
+                        double  yawAverage = computeYawAverage( yawSamples );
+                        yawSamples.erase( yawSamples.begin(), yawSamples.begin() + 10 );
+
+                        double delta = yawAverage - m_parseValueYaw;
+
+                        log.information(
+                            Poco::Logger::format(
+                                "ImuReader average yaw: $0 (delta: $1)",
+                                Poco::NumberFormatter::format( yawAverage ),
+                                Poco::NumberFormatter::format( delta ) ) );
+                    }
+
+                    // new values available
                     if ( ++valuesAvailableCount == 10 )
                     {
                         valuesAvailableCount = 0;