ground/openpilotgcs/src/plugins/gpsdisplay/telemetryparser.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       telemetryparser.cpp
   5  * @author     Edouard Lafargue & the OpenPilot team Copyright (C) 2010.
   6  * @addtogroup GCSPlugins GCS Plugins
   7  * @{
   8  * @addtogroup GPSGadgetPlugin GPS Gadget Plugin
   9  * @{
  10  * @brief A gadget that displays GPS status and enables basic configuration
  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 "telemetryparser.h"
  30 #include <math.h>
  31 #include <QDebug>
  32 #include <QStringList>
  33
  34
  35 /**
  36  * Initialize the parser
  37  */
  38 TelemetryParser::TelemetryParser(QObject *parent) : GPSParser(parent)
  39 {
  40     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
  41     UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
  42     UAVDataObject *gpsObj = dynamic_cast<UAVDataObject *>(objManager->getObject("GPSPositionSensor"));
  43
  44     if (gpsObj != NULL) {
  45         connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateGPS(UAVObject *)));
  46     } else {
  47         qDebug() << "Error: Object is unknown (GPSPositionSensor).";
  48     }
  49
  50     gpsObj = dynamic_cast<UAVDataObject *>(objManager->getObject("GPSTime"));
  51     if (gpsObj != NULL) {
  52         connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateTime(UAVObject *)));
  53     } else {
  54         qDebug() << "Error: Object is unknown (GPSTime).";
  55     }
  56
  57     gpsObj = dynamic_cast<UAVDataObject *>(objManager->getObject("GPSSatellites"));
  58     if (gpsObj != NULL) {
  59         connect(gpsObj, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateSats(UAVObject *)));
  60     }
  61 }
  62
  63 TelemetryParser::~TelemetryParser()
  64 {}
  65
  66
  67 void TelemetryParser::updateGPS(UAVObject *object1)
  68 {
  69     UAVObjectField *field = object1->getField(QString("Satellites"));
  70     emit sv(field->getValue().toInt());
  71
  72     double lat = object1->getField(QString("Latitude"))->getDouble();
  73     double lon = object1->getField(QString("Longitude"))->getDouble();
  74     double alt = object1->getField(QString("Altitude"))->getDouble();
  75
  76     lat *= 1E-7;
  77     lon *= 1E-7;
  78     emit position(lat, lon, alt);
  79
  80     double hdg  = object1->getField(QString("Heading"))->getDouble();
  81     double spd  = object1->getField(QString("Groundspeed"))->getDouble();
  82     emit speedheading(spd, hdg);
  83
  84     QString fix = object1->getField(QString("Status"))->getValue().toString();
  85     emit fixtype(fix);
  86
  87     double hdop = object1->getField(QString("HDOP"))->getDouble();
  88     double vdop = object1->getField(QString("VDOP"))->getDouble();
  89     double pdop = object1->getField(QString("PDOP"))->getDouble();
  90     emit dop(hdop, vdop, pdop);
  91 }
  92
  93 void TelemetryParser::updateTime(UAVObject *object1)
  94 {
  95     double hour   = object1->getField(QString("Hour"))->getDouble();
  96     double minute = object1->getField(QString("Minute"))->getDouble();
  97     double second = object1->getField(QString("Second"))->getDouble();
  98     double time   = second + minute * 100 + hour * 10000;
  99     double year   = object1->getField(QString("Year"))->getDouble();
 100     double month  = object1->getField(QString("Month"))->getDouble();
 101     double day    = object1->getField(QString("Day"))->getDouble();
 102     double date   = day + month * 100 + year * 10000;
 103     emit datetime(date, time);
 104 }
 105
 106 /**
 107    Updates the satellite constellation.
 108
 109    Not really optimized for now, we should only send updates for the stas
 110    which have changed instead of updating everything... That said, Qt is supposed
 111    to be able to optimize redraws anyway.
 112  */
 113 void TelemetryParser::updateSats(UAVObject *object1)
 114 {
 115     UAVObjectField *prn       = object1->getField(QString("PRN"));
 116     UAVObjectField *elevation = object1->getField(QString("Elevation"));
 117     UAVObjectField *azimuth   = object1->getField(QString("Azimuth"));
 118     UAVObjectField *snr       = object1->getField(QString("SNR"));
 119
 120     for (unsigned int i = 0; i < prn->getNumElements(); i++) {
 121         emit satellite(i, prn->getValue(i).toInt(), elevation->getValue(i).toInt(),
 122                        azimuth->getValue(i).toInt(), snr->getValue(i).toInt());
 123     }
 124 }