Merge branch 'corvuscorax/Smoothed_Scope' into next

Conflicts:
	ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp

ground/openpilotgcs/src/plugins/scope/plotdata.cpp

@@ -49,9 +49,14 @@ PlotData::PlotData(QString p_uavObject, QString p_uavField)
 
     xData = new QVector<double>();
     yData = new QVector<double>();
+    yDataHistory = new QVector<double>();
 
     curve = 0;
     scalePower = 0;
+    interpolationSamples = 1;
+    interpolationSum = 0.0f;
+    correctionSum = 0.0f;
+    correctionCount = 0;
     yMinimum = 0;
     yMaximum = 0;
 
@@ -78,6 +83,7 @@ PlotData::~PlotData()
 {
     delete xData;
     delete yData;
+    delete yDataHistory;
 }
 
 
@@ -91,7 +97,24 @@ bool SequencialPlotData::append(UAVObject* obj)
         if (field) {
 
             //Shift data forward and put the new value at the front
-            yData->append( 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();
             } else
@@ -117,8 +140,25 @@ bool ChronoPlotData::append(UAVObject* obj)
             //Put the new value at the front
             QDateTime NOW = QDateTime::currentDateTime();
 
+            // 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 = valueAsDouble(obj, field) * pow(10, scalePower);
+            double valueY = interpolationSum/yDataHistory->size();
             xData->append(valueX);
             yData->append(valueY);
 

ground/openpilotgcs/src/plugins/scope/plotdata.h

@@ -72,12 +72,17 @@ public:
     QString uavSubField;
     bool haveSubField;
     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;
     QwtPlotCurve* curve;
     QVector<double>* xData;
     QVector<double>* yData;
+    QVector<double>* yDataHistory;
 
     virtual bool append(UAVObject* obj) = 0;
     virtual PlotType plotType() = 0;

ground/openpilotgcs/src/plugins/scope/scopegadget.cpp

@@ -61,12 +61,14 @@ void ScopeGadget::loadConfiguration(IUAVGadgetConfiguration* config)
         QString uavObject = plotCurveConfig->uavObject;
         QString uavField = plotCurveConfig->uavField;
         int scale = plotCurveConfig->yScalePower;
+        int interpolation = plotCurveConfig->yInterpolationSamples;
         QRgb color = plotCurveConfig->color;
 
         widget->addCurvePlot(
                 uavObject,
                 uavField,
                 scale,
+		interpolation,
                 QPen(  QBrush(QColor(color),Qt::SolidPattern),
 //					   (qreal)2,
 					   (qreal)1,

ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.cpp

@@ -65,6 +65,8 @@ ScopeGadgetConfiguration::ScopeGadgetConfiguration(QString classId, QSettings* q
             color = qSettings->value("color").value<QRgb>();
             plotCurveConf->color = color;
             plotCurveConf->yScalePower = qSettings->value("yScalePower").toInt();
+            plotCurveConf->yInterpolationSamples = qSettings->value("yInterpolationSamples").toInt();
+            if (!plotCurveConf->yInterpolationSamples) plotCurveConf->yInterpolationSamples = 1; // fallback for backward compatibility with earlier versions
             plotCurveConf->yMinimum = qSettings->value("yMinimum").toDouble();
             plotCurveConf->yMaximum = qSettings->value("yMaximum").toDouble();
 
@@ -118,6 +120,7 @@ IUAVGadgetConfiguration *ScopeGadgetConfiguration::clone()
         newPlotCurveConf->uavField = currentPlotCurveConf->uavField;
         newPlotCurveConf->color = currentPlotCurveConf->color;
         newPlotCurveConf->yScalePower = currentPlotCurveConf->yScalePower;
+        newPlotCurveConf->yInterpolationSamples = currentPlotCurveConf->yInterpolationSamples;
         newPlotCurveConf->yMinimum = currentPlotCurveConf->yMinimum;
         newPlotCurveConf->yMaximum = currentPlotCurveConf->yMaximum;
 
@@ -157,6 +160,7 @@ void ScopeGadgetConfiguration::saveConfig(QSettings* qSettings) const {
         qSettings->setValue("uavField",  plotCurveConf->uavField);
         qSettings->setValue("color",  plotCurveConf->color);
         qSettings->setValue("yScalePower",  plotCurveConf->yScalePower);
+        qSettings->setValue("yInterpolationSamples",  plotCurveConf->yInterpolationSamples);
         qSettings->setValue("yMinimum",  plotCurveConf->yMinimum);
         qSettings->setValue("yMaximum",  plotCurveConf->yMaximum);
 

ground/openpilotgcs/src/plugins/scope/scopegadgetconfiguration.h

@@ -41,6 +41,7 @@ struct PlotCurveConfiguration
     QString uavField;
     int yScalePower; //This is the power to which each value must be raised
     QRgb color;
+    int yInterpolationSamples;
     double yMinimum;
     double yMaximum;
 };

ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.cpp

@@ -101,9 +101,10 @@ QWidget* ScopeGadgetOptionsPage::createPage(QWidget *parent)
         QString uavObject = plotData->uavObject;
         QString uavField = plotData->uavField;
         int scale = plotData->yScalePower;
+        int interpolation = plotData->yInterpolationSamples;
         QVariant varColor = plotData->color;
 
-        addPlotCurveConfig(uavObject,uavField,scale,varColor);
+        addPlotCurveConfig(uavObject,uavField,scale,interpolation,varColor);
     }
 
     if(m_config->plotCurveConfigs().count() > 0)
@@ -163,6 +164,10 @@ void ScopeGadgetOptionsPage::setYAxisWidgetFromPlotCurve()
     int rgb = varColor.toInt(&parseOK);
 
     setButtonColor(QColor((QRgb)rgb));
+
+    int interpolation = listItem->data(Qt::UserRole + 4).toInt(&parseOK);
+    if(!parseOK) interpolation = 1;
+    options_page->spnInterpolationSamples->setValue(interpolation);
 }
 
 void ScopeGadgetOptionsPage::setButtonColor(const QColor &color)
@@ -236,6 +241,10 @@ void ScopeGadgetOptionsPage::apply()
         else
             newPlotCurveConfigs->color = (QRgb)rgb;
 	
+	newPlotCurveConfigs->yInterpolationSamples = listItem->data(Qt::UserRole + 4).toInt(&parseOK);
+        if(!parseOK)
+            newPlotCurveConfigs->yInterpolationSamples = 1;
+
         plotCurveConfigs.append(newPlotCurveConfigs);
     }
 
@@ -261,6 +270,7 @@ void ScopeGadgetOptionsPage::on_btnAddCurve_clicked()
     if(!parseOK)
        scale = 0;
 
+    int interpolation = options_page->spnInterpolationSamples->value();
 
     QVariant varColor = (int)QColor(options_page->btnColor->text()).rgb();
 
@@ -270,27 +280,27 @@ void ScopeGadgetOptionsPage::on_btnAddCurve_clicked()
        options_page->lstCurves->currentItem()->text() == uavObject + "." + uavField)
     {
         QListWidgetItem *listWidgetItem = options_page->lstCurves->currentItem();
-        setCurvePlotProperties(listWidgetItem,uavObject,uavField,scale,varColor);
+        setCurvePlotProperties(listWidgetItem,uavObject,uavField,scale,interpolation,varColor);
     }else
     {
-        addPlotCurveConfig(uavObject,uavField,scale,varColor);
+        addPlotCurveConfig(uavObject,uavField,scale,interpolation,varColor);
 
         options_page->lstCurves->setCurrentRow(options_page->lstCurves->count() - 1);
     }
 }
 
-void ScopeGadgetOptionsPage::addPlotCurveConfig(QString uavObject, QString uavField, int scale, QVariant varColor)
+void ScopeGadgetOptionsPage::addPlotCurveConfig(QString uavObject, QString uavField, int scale, int interpolation, QVariant varColor)
 {
     //Add a new curve config to the list
     QString listItemDisplayText = uavObject + "." + uavField;
     options_page->lstCurves->addItem(listItemDisplayText);
     QListWidgetItem *listWidgetItem = options_page->lstCurves->item(options_page->lstCurves->count() - 1);
 
-    setCurvePlotProperties(listWidgetItem,uavObject,uavField,scale,varColor);
+    setCurvePlotProperties(listWidgetItem,uavObject,uavField,scale,interpolation,varColor);
 
 }
 
-void ScopeGadgetOptionsPage::setCurvePlotProperties(QListWidgetItem *listWidgetItem,QString uavObject, QString uavField, int scale, QVariant varColor)
+void ScopeGadgetOptionsPage::setCurvePlotProperties(QListWidgetItem *listWidgetItem,QString uavObject, QString uavField, int scale, int interpolation, QVariant varColor)
 {
     bool parseOK = false;
 
@@ -306,6 +316,7 @@ void ScopeGadgetOptionsPage::setCurvePlotProperties(QListWidgetItem *listWidgetI
     listWidgetItem->setData(Qt::UserRole + 1,QVariant(uavField));
     listWidgetItem->setData(Qt::UserRole + 2,QVariant(scale));
     listWidgetItem->setData(Qt::UserRole + 3,varColor);
+    listWidgetItem->setData(Qt::UserRole + 4,QVariant(interpolation));
 }
 
 /*!

ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.h

@@ -66,8 +66,8 @@ private:
     Ui::ScopeGadgetOptionsPage *options_page;
     ScopeGadgetConfiguration *m_config;
 
-    void addPlotCurveConfig(QString uavObject, QString uavField, int scale, QVariant varColor);
+    void addPlotCurveConfig(QString uavObject, QString uavField, int scale, int interpolation, QVariant varColor);
-    void setCurvePlotProperties(QListWidgetItem *listWidgetItem, QString uavObject, QString uavField, int scale, QVariant varColor);
+    void setCurvePlotProperties(QListWidgetItem *listWidgetItem, QString uavObject, QString uavField, int scale, int interpolation, QVariant varColor);
     void setYAxisWidgetFromPlotCurve();
     void setButtonColor(const QColor &color);
 

ground/openpilotgcs/src/plugins/scope/scopegadgetoptionspage.ui

@@ -159,6 +159,32 @@
          </property>
         </widget>
        </item>
+       <item row="9" column="0">
+        <widget class="QLabel" name="label_10">
+         <property name="text">
+          <string>Display smoothed interpolation:</string>
+         </property>
+        </widget>
+       </item>
+       <item row="9" column="1">
+        <widget class="QSpinBox" name="spnInterpolationSamples">
+         <property name="suffix">
+          <string> samples</string>
+         </property>
+         <property name="minimum">
+          <number>1</number>
+         </property>
+         <property name="maximum">
+          <number>1001</number>
+         </property>
+         <property name="singleStep">
+          <number>10</number>
+         </property>
+         <property name="value">
+          <number>1</number>
+         </property>
+        </widget>
+       </item>
       </layout>
      </item>
      <item>
@@ -268,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>

ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp

@@ -356,7 +356,7 @@ void ScopeGadgetWidget::setupChronoPlot()
 //	scaleWidget->setMinBorderDist(0, fmw);
 }
 
-void ScopeGadgetWidget::addCurvePlot(QString uavObject, QString uavFieldSubField, int scaleOrderFactor, QPen pen)
+void ScopeGadgetWidget::addCurvePlot(QString uavObject, QString uavFieldSubField, int scaleOrderFactor, int interpolationSamples, QPen pen)
 {
     PlotData* plotData;
 
@@ -369,6 +369,7 @@ void ScopeGadgetWidget::addCurvePlot(QString uavObject, QString uavFieldSubField
 
     plotData->m_xWindowSize = m_xWindowSize;
     plotData->scalePower = scaleOrderFactor;
+    plotData->interpolationSamples = interpolationSamples;
 
     //If the y-bounds are supplied, set them
     if (plotData->yMinimum != plotData->yMaximum)
@@ -446,6 +447,7 @@ void ScopeGadgetWidget::uavObjectReceived(UAVObject* obj)
     foreach(PlotData* plotData, m_curvesData.values()) {
         if (plotData->append(obj)) m_csvLoggingDataUpdated=1;
     }
+    csvLoggingAddData();
 }
 
 void ScopeGadgetWidget::replotNewData()
@@ -609,6 +611,7 @@ int ScopeGadgetWidget::csvLoggingStart()
         m_csvLoggingStartTime = NOW;
         m_csvLoggingHeaderSaved=0;
         m_csvLoggingDataSaved=0;
+        m_csvLoggingBuffer.clear();
         QDir PathCheck(m_csvLoggingPath);
         if (!PathCheck.exists())
         {
@@ -676,20 +679,13 @@ int ScopeGadgetWidget::csvLoggingInsertHeader()
     return 0;
 }
 
-int ScopeGadgetWidget::csvLoggingInsertData()
+int ScopeGadgetWidget::csvLoggingAddData()
 {
     if (!m_csvLoggingStarted) return -1;
-    m_csvLoggingDataSaved=1;
     m_csvLoggingDataValid=0;
     QDateTime NOW = QDateTime::currentDateTime();
     QString tempString;
 
-    if(m_csvLoggingFile.open(QIODevice::WriteOnly | QIODevice::Append)== FALSE)
-    {
-        qDebug() << "Unable to open " << m_csvLoggingFile.fileName() << " for csv logging Data";
-    }
-    else
-    {
     QTextStream ss( &tempString );
     ss << NOW.toString("yyyy-MM-dd") << ", " << NOW.toString("hh:mm:ss.z") << ", " ;
 
@@ -709,19 +705,36 @@ int ScopeGadgetWidget::csvLoggingInsertData()
         }
         else
         {
-                ss  << QString().sprintf("%3.6g",plotData2->yData->last());
+            ss  << QString().sprintf("%3.6g",plotData2->yDataHistory->last());
             m_csvLoggingDataValid=1;
         }
     }
     ss << endl;
     if (m_csvLoggingDataValid)
     {
-            QTextStream ts( &m_csvLoggingFile );
+        QTextStream ts( &m_csvLoggingBuffer );
         ts << tempString;
     }
+
+    return 0;
+}
+
+int ScopeGadgetWidget::csvLoggingInsertData()
+{
+    if (!m_csvLoggingStarted) return -1;
+    m_csvLoggingDataSaved=1;
+
+    if(m_csvLoggingFile.open(QIODevice::WriteOnly | QIODevice::Append)== FALSE)
+    {
+        qDebug() << "Unable to open " << m_csvLoggingFile.fileName() << " for csv logging Data";
+    }
+    else
+    {
+        QTextStream ts( &m_csvLoggingFile );
+        ts << m_csvLoggingBuffer;
         m_csvLoggingFile.close();
     }
-
+    m_csvLoggingBuffer.clear();
 
     return 0;
 }

ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.h

@@ -110,7 +110,7 @@ public:
     int refreshInterval(){return m_refreshInterval;}
 
 
-    void addCurvePlot(QString uavObject, QString uavFieldSubField, int scaleOrderFactor = 0, QPen pen = QPen(Qt::black));
+    void addCurvePlot(QString uavObject, QString uavFieldSubField, int scaleOrderFactor = 0, int interpolationSamples = 1, QPen pen = QPen(Qt::black));
     //void removeCurvePlot(QString uavObject, QString uavField);
     void clearCurvePlots();
     int csvLoggingStart();
@@ -166,11 +166,13 @@ private:
 
     QString m_csvLoggingName;
     QString m_csvLoggingPath;
+    QString m_csvLoggingBuffer;
     QFile m_csvLoggingFile;
 
 	QMutex mutex;
 
     int csvLoggingInsertHeader();
+    int csvLoggingAddData();
     int csvLoggingInsertData();
 
 	void deleteLegend();