From 78278683cced7b17f1b1ccb5978527dca79381dc Mon Sep 17 00:00:00 2001
From: Corvus Corax <corvuscorax@cybertrench.com>
Date: Sat, 26 Nov 2011 22:06:46 +0100
Subject: [PATCH] GCS/Scope: Make smooth interpolation (and internal sum) be
 long term correct despite limited floating point accuracy, but keep constant
 overhead

---
 .../src/plugins/scope/plotdata.cpp            | 31 ++++++++++++++++---
 .../openpilotgcs/src/plugins/scope/plotdata.h |  2 ++
 .../plugins/scope/scopegadgetoptionspage.ui   |  4 +--
 3 files changed, 31 insertions(+), 6 deletions(-)

diff --git a/ground/openpilotgcs/src/plugins/scope/plotdata.cpp b/ground/openpilotgcs/src/plugins/scope/plotdata.cpp
index fdcb75a83..e05bf7a1b 100644
--- a/ground/openpilotgcs/src/plugins/scope/plotdata.cpp
+++ b/ground/openpilotgcs/src/plugins/scope/plotdata.cpp
@@ -55,6 +55,8 @@ PlotData::PlotData(QString p_uavObject, QString p_uavField)
     scalePower = 0;
     interpolationSamples = 1;
     interpolationSum = 0.0f;
+    correctionSum = 0.0f;
+    correctionCount = 0;
     yMinimum = 0;
     yMaximum = 0;
 
@@ -95,12 +97,23 @@ bool SequencialPlotData::append(UAVObject* obj)
         if (field) {
 
             //Shift data forward and put the new value at the front
-            yDataHistory->append( valueAsDouble(obj, field) * pow(10, scalePower));
-            interpolationSum += valueAsDouble(obj, field) * pow(10, scalePower);
+
+            // calculate interpolated (smoothed) value
+            double currentValue = valueAsDouble(obj, field) * pow(10, scalePower);
+            yDataHistory->append( currentValue );
+            interpolationSum += currentValue;
             if(yDataHistory->size() > interpolationSamples) {
                 interpolationSum -= yDataHistory->first();
                 yDataHistory->pop_front();
             }
+            // make sure to correct the sum every interpolationSamples steps to prevent it
+            // from running away due to flouting point rounding errors
+            correctionSum += currentValue;
+            if (++correctionCount >= interpolationSamples) {
+                interpolationSum = correctionSum;
+                correctionSum = 0.0f;
+                correctionCount = 0;
+            }
             yData->append(interpolationSum/yDataHistory->size());
             if (yData->size() > m_xWindowSize) {
                 yData->pop_front();
@@ -127,12 +140,22 @@ bool ChronoPlotData::append(UAVObject* obj)
             //Put the new value at the front
             QDateTime NOW = QDateTime::currentDateTime();
 
-            yDataHistory->append( valueAsDouble(obj, field) * pow(10, scalePower));
-            interpolationSum += valueAsDouble(obj, field) * pow(10, scalePower);
+            // calculate interpolated (smoothed) value
+            double currentValue = valueAsDouble(obj, field) * pow(10, scalePower);
+            yDataHistory->append( currentValue );
+            interpolationSum += currentValue;
             if(yDataHistory->size() > interpolationSamples) {
                 interpolationSum -= yDataHistory->first();
                 yDataHistory->pop_front();
             }
+            // make sure to correct the sum every interpolationSamples steps to prevent it
+            // from running away due to flouting point rounding errors
+            correctionSum += currentValue;
+            if (++correctionCount >= interpolationSamples) {
+                interpolationSum = correctionSum;
+                correctionSum = 0.0f;
+                correctionCount = 0;
+            }
 
             double valueX = NOW.toTime_t() + NOW.time().msec() / 1000.0;
             double valueY = interpolationSum/yDataHistory->size();
diff --git a/ground/openpilotgcs/src/plugins/scope/plotdata.h b/ground/openpilotgcs/src/plugins/scope/plotdata.h
index 0b87c9937..a547f2b91 100644
--- a/ground/openpilotgcs/src/plugins/scope/plotdata.h
+++ b/ground/openpilotgcs/src/plugins/scope/plotdata.h
@@ -74,6 +74,8 @@ public:
     int scalePower; //This is the power to which each value must be raised
     int interpolationSamples;
     double interpolationSum;
+    double correctionSum;
+    int correctionCount;
     double yMinimum;
     double yMaximum;
     double m_xWindowSize;
diff --git a/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.ui b/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.ui
index 147f972f1..b3d22faa0 100644
--- a/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.ui
+++ b/ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.ui
@@ -162,7 +162,7 @@
        <item row="9" column="0">
         <widget class="QLabel" name="label_10">
          <property name="text">
-          <string>Smooth Interpolation:</string>
+          <string>Display smoothed interpolation:</string>
          </property>
         </widget>
        </item>
@@ -294,7 +294,7 @@ Update</string>
        <item>
         <widget class="QCheckBox" name="LoggingEnable">
          <property name="text">
-          <string>Log data to csv file</string>
+          <string>Log data to csv file (not interpolated)</string>
          </property>
         </widget>
        </item>