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 @@
-
- Smooth Interpolation:
+ Display smoothed interpolation:
@@ -294,7 +294,7 @@ Update
-
- Log data to csv file
+ Log data to csv file (not interpolated)