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

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       plotdata.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @addtogroup GCSPlugins GCS Plugins
   7  * @{
   8  * @addtogroup ScopePlugin Scope Gadget Plugin
   9  * @{
  10  * @brief The scope Gadget, graphically plots the states of UAVObjects
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27
  28
  29 #include "plotdata.h"
  30 #include <math.h>
  31 #include <QDebug>
  32
  33 PlotData::PlotData(UAVObject *object, UAVObjectField *field, int element,
  34                    int scaleOrderFactor, int meanSamples, QString mathFunction,
  35                    double plotDataSize, QPen pen, bool antialiased) :
  36     m_scalePower(scaleOrderFactor), m_meanSamples(meanSamples),
  37     m_meanSum(0.0f), m_mathFunction(mathFunction), m_correctionSum(0.0f),
  38     m_correctionCount(0), m_plotDataSize(plotDataSize),
  39     m_object(object), m_field(field), m_element(element),
  40     m_plotCurve(NULL), m_isVisible(true), m_pen(pen), m_isEnumPlot(false)
  41 {
  42     if (m_field->getNumElements() > 1) {
  43         m_elementName = m_field->getElementNames().at(m_element);
  44     }
  45
  46     m_plotName.append(QString("%1.%2").arg(m_object->getName()).arg(m_field->getName()));
  47     if (!m_elementName.isEmpty()) {
  48         m_plotName.append(QString(".%1").arg(m_elementName));
  49     }
  50
  51     if (m_scalePower == 0) {
  52         m_plotName.append(QString(" (%1)").arg(m_field->getUnits()));
  53     } else {
  54         m_plotName.append(QString(" (x10^%1 %2)").arg(m_scalePower).arg(m_field->getUnits()));
  55     }
  56
  57     // Create the curve
  58     m_plotCurve = new QwtPlotCurve(m_plotName);
  59
  60     if (antialiased) {
  61         m_plotCurve->setRenderHint(QwtPlotCurve::RenderAntialiased);
  62     }
  63
  64     m_plotCurve->setPen(m_pen);
  65     m_plotCurve->setSamples(m_xDataEntries, m_yDataEntries);
  66     m_isEnumPlot = m_field->getType() == UAVObjectField::ENUM;
  67 }
  68
  69 PlotData::~PlotData()
  70 {
  71     while (!m_enumMarkerList.isEmpty()) {
  72         QwtPlotMarker *marker = m_enumMarkerList.takeFirst();
  73         marker->detach();
  74         delete marker;
  75     }
  76     m_plotCurve->detach();
  77     delete m_plotCurve;
  78 }
  79
  80 bool PlotData::isVisible() const
  81 {
  82     return m_plotCurve->isVisible();
  83 }
  84
  85 void PlotData::setVisible(bool visible)
  86 {
  87     m_plotCurve->setVisible(visible);
  88     visibilityChanged(m_plotCurve);
  89 }
  90
  91 void PlotData::updatePlotData()
  92 {
  93     m_plotCurve->setSamples(m_xDataEntries, m_yDataEntries);
  94 }
  95
  96 void PlotData::clear()
  97 {
  98     m_meanSum = 0.0f;
  99     m_correctionSum   = 0.0f;
 100     m_correctionCount = 0;
 101     m_xDataEntries.clear();
 102     m_yDataEntries.clear();
 103     while (!m_enumMarkerList.isEmpty()) {
 104         QwtPlotMarker *marker = m_enumMarkerList.takeFirst();
 105         marker->detach();
 106         delete marker;
 107     }
 108     if (wantsInitialData()) {
 109         append(m_object);
 110     }
 111 }
 112
 113 bool PlotData::hasData() const
 114 {
 115     if (!m_isEnumPlot) {
 116         return !m_xDataEntries.isEmpty();
 117     } else {
 118         return !m_enumMarkerList.isEmpty();
 119     }
 120 }
 121
 122 QString PlotData::lastDataAsString()
 123 {
 124     if (!m_isEnumPlot) {
 125         return QString().sprintf("%3.10g", m_yDataEntries.last());
 126     } else {
 127         return m_enumMarkerList.last()->title().text();
 128     }
 129 }
 130
 131 void PlotData::attach(QwtPlot *plot)
 132 {
 133     m_plotCurve->attach(plot);
 134 }
 135
 136 void PlotData::visibilityChanged(QwtPlotItem *item)
 137 {
 138     if (m_plotCurve == item) {
 139         foreach(QwtPlotMarker * marker, m_enumMarkerList) {
 140             m_plotCurve->isVisible() ? marker->attach(m_plotCurve->plot()) : marker->detach();
 141         }
 142     }
 143 }
 144
 145 void PlotData::calcMathFunction(double currentValue)
 146 {
 147     // Put the new value at the back
 148     m_yDataHistory.append(currentValue);
 149
 150     // calculate average value
 151     m_meanSum += currentValue;
 152     if (m_yDataHistory.size() > m_meanSamples) {
 153         m_meanSum -= m_yDataHistory.first();
 154         m_yDataHistory.pop_front();
 155     }
 156     // make sure to correct the sum every meanSamples steps to prevent it
 157     // from running away due to floating point rounding errors
 158     m_correctionSum += currentValue;
 159     if (++m_correctionCount >= m_meanSamples) {
 160         m_meanSum = m_correctionSum;
 161         m_correctionSum = 0.0f;
 162         m_correctionCount = 0;
 163     }
 164
 165     double boxcarAvg = m_meanSum / m_yDataHistory.size();
 166     if (m_mathFunction == "Standard deviation") {
 167         // Calculate square of sample standard deviation, with Bessel's correction
 168         double stdSum = 0;
 169         for (int i = 0; i < m_yDataHistory.size(); i++) {
 170             stdSum += pow(m_yDataHistory.at(i) - boxcarAvg, 2) / (m_meanSamples - 1);
 171         }
 172         m_yDataEntries.append(sqrt(stdSum));
 173     } else {
 174         m_yDataEntries.append(boxcarAvg);
 175     }
 176 }
 177
 178 QwtPlotMarker *PlotData::createMarker(QString value)
 179 {
 180     QwtPlotMarker *marker = new QwtPlotMarker(value);
 181
 182     marker->setZ(10);
 183     QwtText label(QString(" %1 ").arg(value));
 184     label.setColor(QColor(Qt::black));
 185     label.setBorderPen(QPen(m_pen.color(), 1));
 186     label.setBorderRadius(2);
 187     QColor labelBackColor = QColor(Qt::white);
 188     labelBackColor.setAlpha(200);
 189     label.setBackgroundBrush(labelBackColor);
 190     QFont fnt(label.font());
 191     fnt.setPointSize(8);
 192     label.setFont(fnt);
 193     marker->setLabel(label);
 194     marker->setTitle(value);
 195     marker->setLabelOrientation(Qt::Vertical);
 196     marker->setLabelAlignment(Qt::AlignBottom);
 197     marker->setLineStyle(QwtPlotMarker::VLine);
 198     marker->setLinePen(QPen(m_pen.color(), 1, Qt::DashDotLine));
 199     return marker;
 200 }
 201
 202 bool SequentialPlotData::append(UAVObject *obj)
 203 {
 204     if (obj == NULL) {
 205         obj = m_object;
 206     }
 207
 208     if (m_object == obj && m_field) {
 209         if (!m_isEnumPlot) {
 210             double currentValue = m_field->getValue(m_element).toDouble() * pow(10, m_scalePower);
 211
 212             // Perform scope math, if necessary
 213             if (m_mathFunction == "Boxcar average" || m_mathFunction == "Standard deviation") {
 214                 calcMathFunction(currentValue);
 215             } else {
 216                 m_yDataEntries.append(currentValue);
 217             }
 218
 219             if (m_yDataEntries.size() > m_plotDataSize) {
 220                 // If new data overflows the window, remove old data...
 221                 m_yDataEntries.pop_front();
 222             } else {
 223                 // ...otherwise, add a new y point at position xData
 224                 m_xDataEntries.insert(m_xDataEntries.size(), m_xDataEntries.size());
 225             }
 226             return true;
 227         } else {
 228             // Enum markers
 229             QString value = m_field->getValue(m_element).toString();
 230
 231             QwtPlotMarker *marker = m_enumMarkerList.isEmpty() ? NULL : m_enumMarkerList.last();
 232             if (!marker || marker->title() != value) {
 233                 marker = createMarker(value);
 234                 marker->setXValue(m_enumMarkerList.size());
 235
 236                 if (m_plotCurve->isVisible()) {
 237                     marker->attach(m_plotCurve->plot());
 238                 }
 239                 m_enumMarkerList.append(marker);
 240             }
 241         }
 242     }
 243     return false;
 244 }
 245
 246 bool ChronoPlotData::append(UAVObject *obj)
 247 {
 248     if (obj == NULL) {
 249         obj = m_object;
 250     }
 251
 252     if (m_object == obj && m_field) {
 253         // Get the field of interest
 254         // THINK ABOUT REIMPLEMENTING THIS TO SHOW UAVO TIME, NOT SYSTEM TIME
 255         QDateTime NOW = QDateTime::currentDateTime();
 256
 257         double xValue = NOW.toTime_t() + NOW.time().msec() / 1000.0;
 258         if (!m_isEnumPlot) {
 259             double currentValue = m_field->getValue(m_element).toDouble() * pow(10, m_scalePower);
 260
 261             // Perform scope math, if necessary
 262             if (m_mathFunction == "Boxcar average" || m_mathFunction == "Standard deviation") {
 263                 calcMathFunction(currentValue);
 264             } else {
 265                 m_yDataEntries.append(currentValue);
 266             }
 267
 268             m_xDataEntries.append(xValue);
 269         } else {
 270             // Enum markers
 271             QString value = m_field->getValue(m_element).toString();
 272
 273             QwtPlotMarker *marker = m_enumMarkerList.isEmpty() ? NULL : m_enumMarkerList.last();
 274             if (!marker || marker->title() != value) {
 275                 marker = createMarker(value);
 276                 marker->setXValue(xValue);
 277
 278                 if (m_plotCurve->isVisible()) {
 279                     marker->attach(m_plotCurve->plot());
 280                 }
 281                 m_enumMarkerList.append(marker);
 282             }
 283         }
 284         removeStaleData();
 285         return true;
 286     }
 287     return false;
 288 }
 289
 290 void ChronoPlotData::removeStaleData()
 291 {
 292     while (!m_xDataEntries.isEmpty() &&
 293            (m_xDataEntries.last() - m_xDataEntries.first()) > m_plotDataSize) {
 294         m_yDataEntries.pop_front();
 295         m_xDataEntries.pop_front();
 296     }
 297     while (!m_enumMarkerList.isEmpty() &&
 298            (m_enumMarkerList.last()->xValue() - m_enumMarkerList.first()->xValue()) > m_plotDataSize) {
 299         QwtPlotMarker *marker = m_enumMarkerList.takeFirst();
 300         marker->detach();
 301         delete marker;
 302     }
 303 }