diff --git a/.cproject b/.cproject new file mode 100644 index 0000000000000000000000000000000000000000..43f99020af95d6f5d0d494e76fd4774c485bbc46 --- /dev/null +++ b/.cproject @@ -0,0 +1,148 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + mingw32-make + + release + false + false + true + + + mingw32-make + + debug + false + false + true + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/.project b/.project new file mode 100644 index 0000000000000000000000000000000000000000..fc9b8c2de7d855347764478762ca7b92aed604e6 --- /dev/null +++ b/.project @@ -0,0 +1,88 @@ + + + qgroundcontrol + + + + + + com.trolltech.qtcppproject.QtMakefileGenerator + + + + + org.eclipse.cdt.make.core.makeBuilder + clean,full,incremental, + + + org.eclipse.cdt.core.errorOutputParser + org.eclipse.cdt.core.GASErrorParser;org.eclipse.cdt.core.GCCErrorParser;org.eclipse.cdt.core.GLDErrorParser;org.eclipse.cdt.core.GmakeErrorParser;org.eclipse.cdt.core.VCErrorParser;org.eclipse.cdt.core.CWDLocator;org.eclipse.cdt.core.MakeErrorParser; + + + org.eclipse.cdt.make.core.append_environment + true + + + org.eclipse.cdt.make.core.build.arguments + + + + org.eclipse.cdt.make.core.build.command + mingw32-make + + + org.eclipse.cdt.make.core.build.target.auto + debug + + + org.eclipse.cdt.make.core.build.target.clean + clean + + + org.eclipse.cdt.make.core.build.target.inc + debug + + + org.eclipse.cdt.make.core.enableAutoBuild + false + + + org.eclipse.cdt.make.core.enableCleanBuild + true + + + org.eclipse.cdt.make.core.enableFullBuild + true + + + org.eclipse.cdt.make.core.enabledIncrementalBuild + true + + + org.eclipse.cdt.make.core.environment + QMAKESPEC=win32-g++|PATH=C:\\Qt\\2010.04\\qt\\bin;${env_var:PATH}| + + + org.eclipse.cdt.make.core.stopOnError + false + + + org.eclipse.cdt.make.core.useDefaultBuildCmd + false + + + + + org.eclipse.cdt.make.core.ScannerConfigBuilder + + + + + + org.eclipse.cdt.core.cnature + org.eclipse.cdt.core.ccnature + org.eclipse.cdt.make.core.makeNature + org.eclipse.cdt.make.core.ScannerConfigNature + com.trolltech.qtcppproject.QtNature + + diff --git a/images/style-mission.css b/images/style-mission.css index ceacf759608263b76ecb79a732be8246eb9dd51e..0eedac2194c997d897b99950a55c8f2caf195444 100644 --- a/images/style-mission.css +++ b/images/style-mission.css @@ -18,54 +18,54 @@ margin-top: 1ex; /* leave space at the top for the title */ QCheckBox { /*background-color: #252528;*/ color: #DDDDDF; -} - -QCheckBox::indicator { - border: 1px solid #777777; - border-radius: 2px; - color: #DDDDDF; - width: 10px; - height: 10px; -} - -QLineEdit { -border: 1px solid #777777; - border-radius: 2px; -} - -QTextEdit { -border: 1px solid #777777; - border-radius: 2px; -} - -QPlainTextEdit { -border: 1px solid #777777; - border-radius: 2px; -} - -QComboBox { -border: 1px solid #777777; - border-radius: 2px; - } - - QCheckBox::indicator:checked { - background-color: #555555; - } - - QCheckBox::indicator:checked:hover { - background-color: #555555; - } - - QCheckBox::indicator:checked:pressed { - background-color: #555555; - } - - QCheckBox::indicator:indeterminate:hover { - image: url(:/images/checkbox_indeterminate_hover.png); - } - - QCheckBox::indicator:indeterminate:pressed { - image: url(:/images/checkbox_indeterminate_pressed.png); +} + +QCheckBox::indicator { + border: 1px solid #777777; + border-radius: 2px; + color: #DDDDDF; + width: 10px; + height: 10px; +} + +QLineEdit { +border: 1px solid #777777; + border-radius: 2px; +} + +QTextEdit { +border: 1px solid #777777; + border-radius: 2px; +} + +QPlainTextEdit { +border: 1px solid #777777; + border-radius: 2px; +} + +QComboBox { +border: 1px solid #777777; + border-radius: 2px; + } + + QCheckBox::indicator:checked { + background-color: #555555; + } + + QCheckBox::indicator:checked:hover { + background-color: #555555; + } + + QCheckBox::indicator:checked:pressed { + background-color: #555555; + } + + QCheckBox::indicator:indeterminate:hover { + image: url(:/images/checkbox_indeterminate_hover.png); + } + + QCheckBox::indicator:indeterminate:pressed { + image: url(:/images/checkbox_indeterminate_pressed.png); } QGroupBox::title { @@ -73,7 +73,7 @@ border: 1px solid #777777; subcontrol-position: top center; /* position at the top center */ margin: 0 3px 0px 3px; padding: 0 3px 0px 0px; - font: bold 8px; + font: bold 8px; color: #DDDDDF; } @@ -89,7 +89,7 @@ QDockWidget::close-button, QDockWidget::float-button { QDockWidget::title { text-align: left; - background: #121214; + background: #121214; color: #4A4A4F; padding-left: 5px; height: 10px; @@ -102,46 +102,46 @@ QSeparator { QSpinBox { - min-height: 14px; - max-height: 18px; - border: 1px solid #4A4A4F; + min-height: 14px; + max-height: 18px; + border: 1px solid #4A4A4F; border-radius: 5px; -} - -QSpinBox::up-button { - subcontrol-origin: border; - subcontrol-position: top right; /* position at the top right corner */ - border-image: url(:/images/actions/go-up.svg) 1; - border-width: 1px; -} -QSpinBox::down-button { - subcontrol-origin: border; - subcontrol-position: bottom right; /* position at the top right corner */ - border-image: url(:/images/actions/go-down.svg) 1; - border-width: 1px; -} - -QDoubleSpinBox { - min-height: 14px; - max-height: 18px; - border: 1px solid #4A4A4F; - border-radius: 5px; -} - -QDoubleSpinBox::up-button { - subcontrol-origin: border; - subcontrol-position: top right; /* position at the top right corner */ - border-image: url(:/images/actions/go-up.svg) 1; - border-width: 1px; - max-width: 5px; -} -QDoubleSpinBox::down-button { - subcontrol-origin: border; - subcontrol-position: bottom right; /* position at the top right corner */ - border-image: url(:/images/actions/go-down.svg) 1; - border-width: 1px; - max-width: 5px; -} +} + +QSpinBox::up-button { + subcontrol-origin: border; + subcontrol-position: top right; /* position at the top right corner */ + border-image: url(:/images/actions/go-up.svg) 1; + border-width: 1px; +} +QSpinBox::down-button { + subcontrol-origin: border; + subcontrol-position: bottom right; /* position at the top right corner */ + border-image: url(:/images/actions/go-down.svg) 1; + border-width: 1px; +} + +QDoubleSpinBox { + min-height: 14px; + max-height: 18px; + border: 1px solid #4A4A4F; + border-radius: 5px; +} + +QDoubleSpinBox::up-button { + subcontrol-origin: border; + subcontrol-position: top right; /* position at the top right corner */ + border-image: url(:/images/actions/go-up.svg) 1; + border-width: 1px; + max-width: 5px; +} +QDoubleSpinBox::down-button { + subcontrol-origin: border; + subcontrol-position: bottom right; /* position at the top right corner */ + border-image: url(:/images/actions/go-down.svg) 1; + border-width: 1px; + max-width: 5px; +} QPushButton { font-weight: bold; @@ -155,7 +155,7 @@ QPushButton { } QPushButton:checked { - background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #101010, stop: 1 #404040); + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #404040, stop: 1 #808080); } QPushButton:pressed { @@ -170,8 +170,8 @@ QToolButton { border: 2px solid #4A4A4F; border-radius: 5px; background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #232228, stop: 1 #020208); -} - +} + QToolButton:checked { background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #090909, stop: 1 #353535); } @@ -228,7 +228,7 @@ QPushButton:pressed#killButton { border-radius: 5px; } -QPushButton#controlButton { +QPushButton#controlButton { min-height: 25px; background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #A0AE00, stop: 1 #909E00); } @@ -268,4 +268,4 @@ QProgressBar::chunk#speedBar { QProgressBar::chunk#thrustBar { background-color: orange; -} +} diff --git a/lib/QMapControl/src/geometry.cpp b/lib/QMapControl/src/geometry.cpp index e9319e6151d84d6f681246abae3bb541f5cd020b..62f45a75d7cfc07a2377fcba42f407c61c36583d 100644 --- a/lib/QMapControl/src/geometry.cpp +++ b/lib/QMapControl/src/geometry.cpp @@ -29,9 +29,9 @@ namespace qmapcontrol Geometry::Geometry(QString name) : GeometryType("Geometry"), myparentGeometry(0), mypen(0), visible(true), myname(name) { + myIndex = name.toInt(0,10); } - Geometry::~Geometry() { } @@ -40,6 +40,11 @@ namespace qmapcontrol { return myname; } + int Geometry::get_myIndex() const + { + return myIndex; + } + Geometry* Geometry::parentGeometry() const { return myparentGeometry; @@ -85,4 +90,5 @@ namespace qmapcontrol { return mypen; } + } diff --git a/lib/QMapControl/src/geometry.h b/lib/QMapControl/src/geometry.h index dca47800cb47dc934460e9fd1bac38d957be6c9c..2aefd955def97d20c30a15644b5fc10ae4420cb1 100644 --- a/lib/QMapControl/src/geometry.h +++ b/lib/QMapControl/src/geometry.h @@ -49,6 +49,7 @@ namespace qmapcontrol Q_OBJECT public: explicit Geometry(QString name = QString()); + virtual ~Geometry(); QString GeometryType; @@ -74,6 +75,12 @@ namespace qmapcontrol */ QString name() const; + //! returns the index of this Geometry + /*! + * @return the index of this Geometry + */ + int get_myIndex() const; + //! returns the parent Geometry of this Geometry /*! * A LineString is a composition of many Points. This methods returns the parent (the LineString) of a Point @@ -123,6 +130,7 @@ namespace qmapcontrol QPen* mypen; bool visible; QString myname; + int myIndex; void setParentGeometry(Geometry* geom); signals: @@ -149,6 +157,8 @@ namespace qmapcontrol * @param visible if the layer should be visible */ virtual void setVisible(bool visible); + + }; } #endif diff --git a/lib/QMapControl/src/layer.cpp b/lib/QMapControl/src/layer.cpp index 6083b3bec99201b9d321d472ba3f3193f93e9e2a..fa882117593c1efcbf97c6480ca56fea9c88a1d5 100644 --- a/lib/QMapControl/src/layer.cpp +++ b/lib/QMapControl/src/layer.cpp @@ -29,6 +29,7 @@ namespace qmapcontrol Layer::Layer(QString layername, MapAdapter* mapadapter, enum LayerType layertype, bool takeevents) :visible(true), mylayername(layername), mylayertype(layertype), mapAdapter(mapadapter), takeevents(takeevents), myoffscreenViewport(QRect(0,0,0,0)) { + draggingGeometry = false; //qDebug() << "creating new Layer: " << layername << ", type: " << contents; //qDebug() << this->layertype; } diff --git a/lib/QMapControl/src/layermanager.cpp b/lib/QMapControl/src/layermanager.cpp index 286aceefcedfbcf02f98fd4a2a2f504064a89d6d..3e10bb225a580e39e6ed11dc755aef0ced94bb19 100644 --- a/lib/QMapControl/src/layermanager.cpp +++ b/lib/QMapControl/src/layermanager.cpp @@ -366,15 +366,26 @@ namespace qmapcontrol void LayerManager::mouseEvent(const QMouseEvent* evnt) { - QListIterator it(mylayers); - while (it.hasNext()) + // TODO: to review errors generated in the Windows operating system when the QListIterator is used + for(int i=0; iisVisible()) { l->mouseEvent(evnt, mapmiddle_px); } } +// QListIterator it(mylayers); + +// while (it.hasNext()) +// { +// qDebug() << it.next(); +// Layer* l = it.next(); +// if (l->isVisible()) +// { +// l->mouseEvent(evnt, mapmiddle_px); +// } +// } } void LayerManager::updateRequest(QRectF rect) diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index 31044634075b589f7d9801143e9af14ccb9d6438..f9bffcac10adb7110b14920f0d188a735f970167 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -126,7 +126,8 @@ win32 { # Special settings for debug #CONFIG += CONSOLE - INCLUDEPATH += $$BASEDIR\lib\sdl\include #\ + INCLUDEPATH += $$BASEDIR\lib\sdl\include \ + $$BASEDIR\lib\opal\include #\ #\ #"C:\Program Files\Microsoft SDKs\Windows\v7.0\Include" LIBS += -L$$BASEDIR\lib\sdl\win32 \ diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 76595b422d1f8b4f097b0e36718201ec21b54a57..edb59b6b47b19341b5114a314042507fe824f073 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -18,7 +18,7 @@ TARGET = qgroundcontrol BASEDIR = . BUILDDIR = build LANGUAGE = C++ -CONFIG += debug_and_release \ +CONFIG += debug_and_release \ console OBJECTS_DIR = $$BUILDDIR/obj MOC_DIR = $$BUILDDIR/moc @@ -42,8 +42,8 @@ DEPENDPATH += . \ plugins INCLUDEPATH += . \ lib/QMapControl \ - $$BASEDIR/../mavlink/contrib/slugs/include \ - $$BASEDIR/../mavlink/include + $$BASEDIR/../mavlink/include \ + $$BASEDIR/../mavlink/contrib/slugs/include # ../mavlink/include \ # MAVLink/include \ @@ -75,7 +75,8 @@ FORMS += src/ui/MainWindow.ui \ src/ui/QGCFirmwareUpdate.ui \ src/ui/QGCPxImuFirmwareUpdate.ui \ src/ui/QGCDataPlot2D.ui \ - src/ui/QGCRemoteControlView.ui + src/ui/QGCRemoteControlView.ui \ + src/ui/WaypointGlobalView.ui INCLUDEPATH += src \ src/ui \ src/ui/linechart \ @@ -157,7 +158,9 @@ HEADERS += src/MG.h \ src/ui/map/Waypoint2DIcon.h \ src/ui/map/MAV2DIcon.h \ src/ui/map/QGC2DIcon.h \ - src/ui/QGCRemoteControlView.h + src/ui/QGCRemoteControlView.h \ + src/WaypointGlobal.h \ + src/ui/WaypointGlobalView.h SOURCES += src/main.cc \ src/Core.cc \ src/uas/UASManager.cc \ @@ -221,15 +224,29 @@ SOURCES += src/main.cc \ src/ui/map/Waypoint2DIcon.cc \ src/ui/map/MAV2DIcon.cc \ src/ui/map/QGC2DIcon.cc \ - src/ui/QGCRemoteControlView.cc + src/ui/QGCRemoteControlView.cc \ + src/WaypointGlobal.cpp \ + src/ui/WaypointGlobalView.cpp RESOURCES = mavground.qrc # Include RT-LAB Library -win32 { +win32:exists(src/lib/opalrt/OpalApi.h) { + message("Building support for Opal-RT") LIBS += -LC:\OPAL-RT\RT-LAB7.2.4\Common\bin \ -lOpalApi INCLUDEPATH += src/lib/opalrt - SOURCES += src/comm/OpalLink.cc - HEADERS += src/comm/OpalLink.h + HEADERS += src/comm/OpalRT.h \ + src/comm/OpalLink.h \ + src/comm/Parameter.h \ + src/comm/QGCParamID.h \ + src/comm/ParameterList.h \ + src/ui/OpalLinkConfigurationWindow.h + SOURCES += src/comm/OpalRT.cc \ + src/comm/OpalLink.cc \ + src/comm/Parameter.cc \ + src/comm/QGCParamID.cc \ + src/comm/ParameterList.cc \ + src/ui/OpalLinkConfigurationWindow.cc + FORMS += src/ui/OpalLinkSettings.ui DEFINES += OPAL_RT } diff --git a/settings/ParameterList.xml b/settings/ParameterList.xml new file mode 100644 index 0000000000000000000000000000000000000000..3894080f4c43a26c423c7b3cce235ec4cb77a275 --- /dev/null +++ b/settings/ParameterList.xml @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/Core.cc b/src/Core.cc index 482b0deada20e58eef556e89e9e901c2dc629dd7..7035d08c815bdc147efdf64e8a0567461087945f 100644 --- a/src/Core.cc +++ b/src/Core.cc @@ -142,7 +142,6 @@ Core::Core(int &argc, char* argv[]) : QApplication(argc, argv) // Add OpalRT Link, but do not connect OpalLink* opalLink = new OpalLink(); mainWindow->addLink(opalLink); -#warning OPAL LINK NOW AUTO CONNECTING IN CORE.CC #endif // MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(MG::DIR::getSupportFilesDirectory() + "/demo-log.txt"); MAVLinkSimulationLink* simulationLink = new MAVLinkSimulationLink(":/demo-log.txt"); diff --git a/src/Waypoint.h b/src/Waypoint.h index 053f6cbb0f60b1c187ffa8394e306a8b90b54cf6..d8baeba9e04eb181e112e663c425e469693f7153 100644 --- a/src/Waypoint.h +++ b/src/Waypoint.h @@ -59,7 +59,7 @@ public: bool load(QTextStream &loadStream); -private: +protected: quint16 id; float x; float y; diff --git a/src/WaypointGlobal.cpp b/src/WaypointGlobal.cpp new file mode 100644 index 0000000000000000000000000000000000000000..fc5526f1c1af4a735e87a1545d091feba0925b19 --- /dev/null +++ b/src/WaypointGlobal.cpp @@ -0,0 +1,10 @@ +#include "WaypointGlobal.h" + +#include + +WaypointGlobal::WaypointGlobal(const QPointF coordinate): + Waypoint(id, x, y, z, yaw, autocontinue, current, orbit, holdTime) +{ + coordinateWP = coordinate; + +} diff --git a/src/WaypointGlobal.h b/src/WaypointGlobal.h new file mode 100644 index 0000000000000000000000000000000000000000..4a3bc4e94ff188ad8415d77a94bd9c78797658c0 --- /dev/null +++ b/src/WaypointGlobal.h @@ -0,0 +1,29 @@ +#ifndef WAYPOINTGLOBAL_H +#define WAYPOINTGLOBAL_H + +#include "Waypoint.h" +#include + +class WaypointGlobal: public Waypoint { + Q_OBJECT + +public: + WaypointGlobal(const QPointF coordinate); + + public slots: + +// void set_latitud(double latitud); +// void set_longitud(double longitud); +// double get_latitud(); +// double get_longitud(); + +private: + QPointF coordinateWP; + + + + + +}; + +#endif // WAYPOINTGLOBAL_H diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc index 90ff9805e5df7480cad1b72fe10841b644539786..9c10d5d8e29e5c96058a1c2f89b3a85319806c62 100644 --- a/src/comm/OpalLink.cc +++ b/src/comm/OpalLink.cc @@ -35,10 +35,12 @@ OpalLink::OpalLink() : heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE), m_heartbeatsEnabled(true), getSignalsTimer(new QTimer(this)), - getSignalsPeriod(1000), + getSignalsPeriod(10), receiveBuffer(new QQueue()), systemID(1), - componentID(1) + componentID(1), + params(NULL), + opalInstID(101) { start(QThread::LowPriority); @@ -67,29 +69,83 @@ qint64 OpalLink::bytesAvailable() void OpalLink::writeBytes(const char *bytes, qint64 length) { - + /* decode the message */ + mavlink_message_t msg; + mavlink_status_t status; + int decodeSuccess = 0; + for (int i=0; (!(decodeSuccess=mavlink_parse_char(this->getId(), bytes[i], &msg, &status))&& ibegin(); paramIter != params->end(); ++paramIter) + { + mavlink_msg_param_value_pack(systemID, + (*paramIter).getComponentID(), + ¶m, + (*paramIter).getParamID().toInt8_t(), + (static_cast(*paramIter)).getValue(), + params->count(), + params->indexOf(*paramIter)); + receiveMessage(param); + } + + + } + case MAVLINK_MSG_ID_PARAM_SET: + { + +// qDebug() << "OpalLink::writeBytes(): Attempt to set a parameter"; + + mavlink_param_set_t param; + mavlink_msg_param_set_decode(&msg, ¶m); + OpalRT::QGCParamID paramName((char*)param.param_id); + +// qDebug() << "OpalLink::writeBytes():paramName: " << paramName; + + if ((*params).contains(param.target_component, paramName)) + { + OpalRT::Parameter p = (*params)(param.target_component, paramName); +// qDebug() << __FILE__ << ":" << __LINE__ << ": " << p; + // Set the param value in Opal-RT + p.setValue(param.param_value); + + // Get the param value from Opal-RT to make sure it was set properly + mavlink_message_t paramMsg; + mavlink_msg_param_value_pack(systemID, + p.getComponentID(), + ¶mMsg, + p.getParamID().toInt8_t(), + p.getValue(), + params->count(), + params->indexOf(p)); + receiveMessage(paramMsg); + } + } + break; + default: + { + qDebug() << "OpalLink::writeBytes(): Unknown mavlink packet"; + } + } + } } void OpalLink::readBytes() { receiveDataMutex.lock(); - const qint64 maxLength = 2048; - char bytes[maxLength]; - qDebug() << "OpalLink::readBytes(): Reading a message. size of buffer: " << receiveBuffer->count(); - QByteArray message = receiveBuffer->dequeue(); - if (maxLength < message.size()) - { - qDebug() << "OpalLink::readBytes:: Buffer Overflow"; - - memcpy(bytes, message.data(), maxLength); - } - else - { - memcpy(bytes, message.data(), message.size()); - } - - emit bytesReceived(this, message); + emit bytesReceived(this, receiveBuffer->dequeue()); receiveDataMutex.unlock(); } @@ -104,7 +160,9 @@ void OpalLink::receiveMessage(mavlink_message_t message) // If link is connected if (isConnected()) { + receiveDataMutex.lock(); receiveBuffer->enqueue(QByteArray(buffer, len)); + receiveDataMutex.unlock(); readBytes(); } @@ -115,54 +173,97 @@ void OpalLink::heartbeat() if (m_heartbeatsEnabled) { - qDebug() << "OpalLink::heartbeat(): Generate a heartbeat"; mavlink_message_t beat; mavlink_msg_heartbeat_pack(systemID, componentID,&beat, MAV_HELICOPTER, MAV_AUTOPILOT_GENERIC); receiveMessage(beat); } } +void OpalLink::setSignals(double *values) +{ + unsigned short numSignals = 2; + unsigned short logicalId = 1; + unsigned short signalIndex[] = {0,1}; + int returnValue; + returnValue = OpalSetSignals( numSignals, logicalId, signalIndex, values); + if (returnValue != EOK) + { + OpalRT::OpalErrorMsg::displayLastErrorMsg(); + } +} void OpalLink::getSignals() { -// qDebug() << "OpalLink::getSignals(): Attempting to acquire signals"; -// -// -// unsigned long timeout = 0; -// unsigned short acqGroup = 0; //this is actually group 1 in the model -// unsigned short allocatedSignals = NUM_OUTPUT_SIGNALS; -// unsigned short *numSignals = new unsigned short(0); -// double *timestep = new double(0); -// double values[NUM_OUTPUT_SIGNALS] = {}; -// unsigned short *lastValues = new unsigned short(false); -// unsigned short *decimation = new unsigned short(0); -// -// int returnVal = OpalGetSignals(timeout, acqGroup, allocatedSignals, numSignals, timestep, -// values, lastValues, decimation); -// -// if (returnVal == EOK ) -// { -// qDebug() << "OpalLink::getSignals: Timestep=" << *timestep;// << ", Last? " << (bool)(*lastValues); -// } -// else if (returnVal == EAGAIN) -// { -// qDebug() << "OpalLink::getSignals: Data was not ready"; -// } -// // if returnVal == EAGAIN => data just wasn't ready -// else if (returnVal != EAGAIN) -// { -// getSignalsTimer->stop(); -// displayErrorMsg(); -// } -// /* deallocate used memory */ -// -// delete timestep; -// delete lastValues; -// delete lastValues; -// delete decimation; + unsigned long timeout = 0; + unsigned short acqGroup = 0; //this is actually group 1 in the model + unsigned short *numSignals = new unsigned short(0); + double *timestep = new double(0); + double values[OpalRT::NUM_OUTPUT_SIGNALS] = {}; + unsigned short *lastValues = new unsigned short(false); + unsigned short *decimation = new unsigned short(0); + + while (!(*lastValues)) + { + int returnVal = OpalGetSignals(timeout, acqGroup, OpalRT::NUM_OUTPUT_SIGNALS, numSignals, timestep, + values, lastValues, decimation); + + if (returnVal == EOK ) + { + /* Send position info to qgroundcontrol */ + mavlink_message_t local_position; + mavlink_msg_local_position_pack(systemID, componentID, &local_position, + (*timestep)*1000000, + values[OpalRT::X_POS], + values[OpalRT::Y_POS], + values[OpalRT::Z_POS], + values[OpalRT::X_VEL], + values[OpalRT::Y_VEL], + values[OpalRT::Z_VEL]); + receiveMessage(local_position); + + /* send attitude info to qgroundcontrol */ + mavlink_message_t attitude; + mavlink_msg_attitude_pack(systemID, componentID, &attitude, + (*timestep)*1000000, + values[OpalRT::ROLL], + values[OpalRT::PITCH], + values[OpalRT::YAW], + values[OpalRT::ROLL_SPEED], + values[OpalRT::PITCH_SPEED], + values[OpalRT::YAW_SPEED] + ); + receiveMessage(attitude); + + /* send bias info to qgroundcontrol */ + mavlink_message_t bias; + mavlink_msg_nav_filter_bias_pack(systemID, componentID, &bias, + (*timestep)*1000000, + values[OpalRT::B_F_0], + values[OpalRT::B_F_1], + values[OpalRT::B_F_2], + values[OpalRT::B_W_0], + values[OpalRT::B_W_1], + values[OpalRT::B_W_2] + ); + receiveMessage(bias); + } + else if (returnVal != EAGAIN) // if returnVal == EAGAIN => data just wasn't ready + { + getSignalsTimer->stop(); + OpalRT::OpalErrorMsg::displayLastErrorMsg(); + } + } + + /* deallocate used memory */ + + delete numSignals; + delete timestep; + delete lastValues; + delete decimation; } + /* * Administrative @@ -170,7 +271,7 @@ void OpalLink::getSignals() */ void OpalLink::run() { - qDebug() << "OpalLink::run():: Starting the thread"; +// qDebug() << "OpalLink::run():: Starting the thread"; } int OpalLink::getId() @@ -189,8 +290,7 @@ void OpalLink::setName(QString name) emit nameChanged(this->name); } -bool OpalLink::isConnected() { - //qDebug() << "OpalLink::isConnected:: connectState: " << connectState; +bool OpalLink::isConnected() { return connectState; } @@ -201,19 +301,24 @@ bool OpalLink::connect() { short modelState; - /// \todo allow configuration of instid in window -// if (OpalConnect(101, false, &modelState) == EOK) -// { + /// \todo allow configuration of instid in window + if ((OpalConnect(opalInstID, false, &modelState) == EOK) + && (OpalGetSignalControl(0, true) == EOK) + && (OpalGetParameterControl(true) == EOK)) + { connectState = true; + if (params) + delete params; + params = new OpalRT::ParameterList(); emit connected(); heartbeatTimer->start(1000/heartbeatRate); getSignalsTimer->start(getSignalsPeriod); -// } -// else -// { -// connectState = false; -// displayErrorMsg(); -// } + } + else + { + connectState = false; + OpalRT::OpalErrorMsg::displayLastErrorMsg(); + } emit connected(connectState); return connectState; @@ -221,26 +326,16 @@ bool OpalLink::connect() bool OpalLink::disconnect() { - return false; + // OpalDisconnect returns void so its success or failure cannot be tested + OpalDisconnect(); + heartbeatTimer->stop(); + getSignalsTimer->stop(); + connectState = false; + emit connected(connectState); + return true; } -void OpalLink::displayErrorMsg() -{ - setLastErrorMsg(); - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText(lastErrorMsg); - msgBox.exec(); -} -void OpalLink::setLastErrorMsg() -{ -// char buf[512]; -// unsigned short len; -// OpalGetLastErrMsg(buf, sizeof(buf), &len); -// lastErrorMsg.clear(); -// lastErrorMsg.append(buf); -} /* diff --git a/src/comm/OpalLink.h b/src/comm/OpalLink.h index ad0cbe65b3747e42fc76dd17cb3fa7c13fa8e64a..07b685c0b28ac715cf4076cd6ab3a50c00e25045 100644 --- a/src/comm/OpalLink.h +++ b/src/comm/OpalLink.h @@ -32,11 +32,13 @@ This file is part of the QGROUNDCONTROL project #include #include -#include +#include #include #include #include #include +#include + #include "LinkInterface.h" #include "LinkManager.h" @@ -44,31 +46,18 @@ This file is part of the QGROUNDCONTROL project #include "mavlink.h" #include "mavlink_types.h" #include "configuration.h" - +#include "OpalRT.h" +#include "ParameterList.h" +#include "Parameter.h" +#include "QGCParamID.h" +#include "OpalApi.h" #include "errno.h" - - - - -// FIXME -//#include "OpalApi.h" - - - - - #include "string.h" -/* - Configuration info for the model - */ - -#define NUM_OUTPUT_SIGNALS 6 - /** - * @brief Interface to OPAL-RT targets + * @brief Interface to OpalRT targets * - * This is an interface to the Opal-RT hardware-in-the-loop (HIL) simulator. + * This is an interface to the OpalRT hardware-in-the-loop (HIL) simulator. * This class receives MAVLink packets as if it is a true link, but it * interprets the packets internally, and calls the appropriate api functions. * @@ -100,34 +89,32 @@ public: qint64 getBitsSent(); qint64 getBitsReceived(); - bool connect(); - bool disconnect(); - qint64 bytesAvailable(); void run(); -public slots: + int getOpalInstID() {return static_cast(opalInstID);} +public slots: void writeBytes(const char *bytes, qint64 length); - void readBytes(); void heartbeat(); void getSignals(); + void setOpalInstID(int instID) {opalInstID = static_cast(instID);} + protected slots: void receiveMessage(mavlink_message_t message); - - + void setSignals(double *values); protected: QString name; @@ -144,9 +131,6 @@ protected: QMutex statisticsMutex; QMutex receiveDataMutex; - QString lastErrorMsg; - void setLastErrorMsg(); - void displayErrorMsg(); void setName(QString name); @@ -163,7 +147,10 @@ protected: const int systemID; const int componentID; + void getParameterList(); + OpalRT::ParameterList *params; + unsigned short opalInstID; }; #endif // OPALLINK_H diff --git a/src/comm/OpalRT.cc b/src/comm/OpalRT.cc new file mode 100644 index 0000000000000000000000000000000000000000..21917ae2fdf68e1480012aafb905bc4952cb283f --- /dev/null +++ b/src/comm/OpalRT.cc @@ -0,0 +1,26 @@ +#include "OpalRT.h" + +namespace OpalRT +{ + // lastErrorMsg = QString(); + void OpalErrorMsg::displayLastErrorMsg() + { + static QString lastErrorMsg; + setLastErrorMsg(); + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText(lastErrorMsg); + msgBox.exec(); + } + + void OpalErrorMsg::setLastErrorMsg() + { + char* buf = new char[512]; + unsigned short len; + static QString lastErrorMsg; + OpalGetLastErrMsg(buf, sizeof(buf), &len); + lastErrorMsg.clear(); + lastErrorMsg.append(buf); + delete buf; + } +} diff --git a/src/comm/OpalRT.h b/src/comm/OpalRT.h new file mode 100644 index 0000000000000000000000000000000000000000..678c44f177d2ce36416024070f35578fea2c4312 --- /dev/null +++ b/src/comm/OpalRT.h @@ -0,0 +1,112 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Types used for Opal-RT interface configuration + * @author Bryan Godbolt + */ + +#ifndef OPALRT_H +#define OPALRT_H + +#include +#include + +#include "OpalApi.h" + +namespace OpalRT +{ + /** + Configuration info for the model + */ + + const unsigned short NUM_OUTPUT_SIGNALS=39; + + /* ------------------------------ Outputs ------------------------------ + * + * Port 1: Navigation state estimates + * 1 t [s] time elapsed since INS mode started + * 2-4 p^n [m] navigation frame position (N,E,D) + * 5-7 v^n [m/s] navigation frame velocity (N,E,D) + * 8-10 Euler angles [rad] (roll, pitch, yaw) + * 11-13 Angular rates + * 14-16 b_f [m/s^2] accelerometer biases + * 17-19 b_w [rad/s] gyro biases + * + * Port 2: Navigation system status + * 1 mode (0: initialization, 1: INS) + * 2 t_GPS time elapsed since last valid GPS measurement + * 3 solution status (0: SOL_COMPUTED, 1: INSUFFICIENT_OBS) + * 4 position solution type ( 0: NONE, 34: NARROW_FLOAT, + * 49: WIDE_INT, 50: NARROW_INT) + * 5 # obs (number of visible satellites) + * + * Port 3: Covariance matrix diagonal + * 1-15 diagonal elements of estimation error covariance matrix P + */ + enum SignalPort + { + T_ELAPSED, + X_POS, + Y_POS, + Z_POS, + X_VEL, + Y_VEL, + Z_VEL, + ROLL, + PITCH, + YAW, + ROLL_SPEED, + PITCH_SPEED, + YAW_SPEED, + B_F_0, + B_F_1, + B_F_2, + B_W_0, + B_W_1, + B_W_2 + }; + + /** Component IDs of the parameters. Currently they are all 1 becuase there is no advantage + to dividing them between component ids. However this syntax is used so that this can + easily be changed in the future. + */ + enum SubsystemIds + { + NAV_ID = 1, + LOG_ID, + CONTROLLER_ID, + SERVO_OUTPUTS, + SERVO_INPUTS + }; + + class OpalErrorMsg + { + static QString lastErrorMsg; + public: + static void displayLastErrorMsg(); + static void setLastErrorMsg(); + }; +} +#endif // OPALRT_H diff --git a/src/comm/Parameter.cc b/src/comm/Parameter.cc new file mode 100644 index 0000000000000000000000000000000000000000..512982e019d507a98f0f8caa13b95f60aed12a12 --- /dev/null +++ b/src/comm/Parameter.cc @@ -0,0 +1,120 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Implementation of class OpalRT::Parameter + * @author Bryan Godbolt + */ +#include "Parameter.h" +using namespace OpalRT; + +//Parameter::Parameter(char *simulinkPath, char *simulinkName, uint8_t componentID, +// QGCParamID paramID, unsigned short opalID) +// : simulinkPath(new QString(simulinkPath)), +// simulinkName(new QString(simulinkName)), +// componentID(componentID), +// paramID(new QGCParamID(paramID)), +// opalID(opalID) +// +//{ +//} +Parameter::Parameter(QString simulinkPath, QString simulinkName, uint8_t componentID, + QGCParamID paramID, unsigned short opalID) + : simulinkPath(new QString(simulinkPath)), + simulinkName(new QString(simulinkName)), + componentID(componentID), + paramID(new QGCParamID(paramID)), + opalID(opalID) + +{ +} +Parameter::Parameter(const Parameter &other) + : componentID(other.componentID), + opalID(other.opalID) +{ + simulinkPath = new QString(*other.simulinkPath); + simulinkName = new QString(*other.simulinkName); + paramID = new QGCParamID(*other.paramID); +} + +Parameter::~Parameter() +{ + delete simulinkPath; + delete simulinkName; + delete paramID; +} + +bool Parameter::operator ==(const Parameter& other) const +{ + return + (*simulinkPath) == *(other.simulinkPath) + && *simulinkName == *(other.simulinkName) + && componentID == other.componentID + && *paramID == *(other.paramID) + && opalID == other.opalID; + +} + +float Parameter::getValue() +{ + unsigned short allocatedParams = 1; + unsigned short numParams; + unsigned short numValues = 1; + unsigned short returnedNumValues; + double value; + + int returnVal = OpalGetParameters(allocatedParams, &numParams, &opalID, + numValues, &returnedNumValues, &value); + + if (returnVal != EOK) + { + OpalRT::OpalErrorMsg::displayLastErrorMsg(); + return FLT_MAX; + } + + return static_cast(value); +} + +void Parameter::setValue(float val) +{ + unsigned short allocatedParams = 1; + unsigned short numParams; + unsigned short numValues = 1; + unsigned short returnedNumValues; + double value = static_cast(val); + + int returnVal = OpalSetParameters(allocatedParams, &numParams, &opalID, + numValues, &returnedNumValues, &value); + if (returnVal != EOK) + { + //qDebug() << __FILE__ << ":" << __LINE__ << ": Error numer: " << QString::number(returnVal); + OpalErrorMsg::displayLastErrorMsg(); + } +} + +Parameter::operator QString() const +{ + return *simulinkPath + *simulinkName + " " + QString::number(componentID) + + " " + *paramID + " " + QString::number(opalID); +} diff --git a/src/comm/Parameter.h b/src/comm/Parameter.h new file mode 100644 index 0000000000000000000000000000000000000000..695cd8a79d6a24b474f006db54337c1196d1e329 --- /dev/null +++ b/src/comm/Parameter.h @@ -0,0 +1,82 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Parameter Object used to intefrace with an OpalRT Simulink Parameter + \see OpalLink + \see OpalRT::ParameterList + * @author Bryan Godbolt + */ + +#ifndef PARAMETER_H +#define PARAMETER_H + +#include +#include + +#include "mavlink_types.h" +#include "QGCParamID.h" +#include "OpalApi.h" +#include "OpalRT.h" +#include + +namespace OpalRT +{ + class Parameter + { + public: +// Parameter(char *simulinkPath = "", +// char *simulinkName = "", +// uint8_t componentID = 0, +// QGCParamID paramID = QGCParamID(), +// unsigned short opalID = 0); + Parameter(QString simulinkPath = QString(), + QString simulinkName = QString(), + uint8_t componentID = 0, + QGCParamID paramID = QGCParamID(), + unsigned short opalID = 0); + Parameter(const Parameter& other); + ~Parameter(); + + const QGCParamID& getParamID() const {return *paramID;} + void setOpalID(unsigned short opalID) {this->opalID = opalID;} + const QString& getSimulinkPath() {return *simulinkPath;} + const QString& getSimulinkName() {return *simulinkName;} + uint8_t getComponentID() const {return componentID;} + float getValue(); + void setValue(float value); + + bool operator==(const Parameter& other) const; + operator QString() const; + + protected: + QString *simulinkPath; + QString *simulinkName; + uint8_t componentID; + QGCParamID *paramID; + unsigned short opalID; + }; +} + +#endif // PARAMETER_H diff --git a/src/comm/ParameterList.cc b/src/comm/ParameterList.cc new file mode 100644 index 0000000000000000000000000000000000000000..0b1d3762468c953925a21ca7c61fc16408534004 --- /dev/null +++ b/src/comm/ParameterList.cc @@ -0,0 +1,347 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Implementation of class OpalRT::ParameterList + * @author Bryan Godbolt + */ + +#include "ParameterList.h" +using namespace OpalRT; + +ParameterList::ParameterList() + :params(new QMap >), + paramList(new QList >()) +{ + + QDir settingsDir = QDir(qApp->applicationDirPath()); + if (settingsDir.dirName() == "bin") + settingsDir.cdUp(); + settingsDir.cd("settings"); + + QString filename(settingsDir.path() + "/ParameterList.xml"); + if ((QFile::exists(filename)) && open(filename)) + { + /* Populate the map with parameter names. There is no elegant way of doing this so all + parameter paths and names must be known at compile time and defined here. + Note: This function is written in a way that calls a lot of copy constructors and is + therefore not particularly efficient. However since it is only called once memory + and computation time are sacrificed for code clarity when adding and modifying + parameters. + When defining the path, the trailing slash is necessary + */ + // Parameter *p; + // /* Component: Navigation Filter */ + // p = new Parameter("avionics_src/sm_ampro/NAV_FILT_INIT/", + // "Value", + // OpalRT::NAV_ID, + // QGCParamID("NAV_FILT_INIT")); + // (*params)[OpalRT::NAV_ID].insert(p->getParamID(), *p); + // delete p; + // + // p = new Parameter("avionics_src/sm_ampro/Gain/", + // "Gain", + // OpalRT::NAV_ID, + // QGCParamID("TEST_OUTP_GAIN")); + // (*params)[OpalRT::NAV_ID].insert(p->getParamID(), *p); + // delete p; + // + // /* Component: Log Facility */ + // p = new Parameter("avionics_src/sm_ampro/LOG_FILE_ON/", + // "Value", + // OpalRT::LOG_ID, + // QGCParamID("LOG_FILE_ON")); + // (*params)[OpalRT::LOG_ID].insert(p->getParamID(), *p); + // delete p; + + /* Get a list of the available parameters from opal-rt */ + QMap *opalParams = new QMap; + getParameterList(opalParams); + + /* Iterate over the parameters we want to use in qgc and populate their ids */ + QMap >::iterator componentIter; + QMap::iterator paramIter; + QString s; + for (componentIter = params->begin(); componentIter != params->end(); ++componentIter) + { + paramList->append(QList()); + for (paramIter = (*componentIter).begin(); paramIter != (*componentIter).end(); ++paramIter) + { + paramList->last().append(paramIter.operator ->()); + s = (*paramIter).getSimulinkPath() + (*paramIter).getSimulinkName(); + if (opalParams->contains(s)) + { + (*paramIter).setOpalID(opalParams->value(s)); + // qDebug() << __FILE__ << " Line:" << __LINE__ << ": Successfully added " << s; + } + else + { + qWarning() << __FILE__ << " Line:" << __LINE__ << ": " << s << " was not found in param list"; + } + } + } + delete opalParams; + } +} + +ParameterList::~ParameterList() +{ + delete params; + delete paramList; +} + +/** + Get the list of parameters in the simulink model. This function does not require + any prior knowlege of the parameters. It works by first calling OpalGetParameterList to + get the number of paramters, then allocates the required amount of memory and then gets + the paramter list using a second call to OpalGetParameterList. + */ +void ParameterList::getParameterList(QMap *opalParams) +{ + /* inputs */ + unsigned short allocatedParams=0; + unsigned short allocatedPathLen=0; + unsigned short allocatedNameLen=0; + unsigned short allocatedVarLen=0; + + /* outputs */ + unsigned short numParams; + unsigned short *idParam=NULL; + unsigned short maxPathLen; + char **paths=NULL; + unsigned short maxNameLen; + char **names=NULL; + unsigned short maxVarLen; + char **var=NULL; + + int returnValue; + + returnValue = OpalGetParameterList(allocatedParams, &numParams, idParam, + allocatedPathLen, &maxPathLen, paths, + allocatedNameLen, &maxNameLen, names, + allocatedVarLen, &maxVarLen, var); + if (returnValue!=E2BIG) + { +// OpalRT::setLastErrorMsg(); + OpalRT::OpalErrorMsg::displayLastErrorMsg(); + return; + } + + // allocate memory for parameter list + + idParam = new unsigned short[numParams]; + allocatedParams = numParams; + + paths = new char*[numParams]; + for (int i=0; iinsert(path+name, idParam[i]); + else + opalParams->insert(path+'/'+name, idParam[i]); + } +// Dump out the list of parameters +// QMap::const_iterator paramPrint; +// for (paramPrint = opalParams->begin(); paramPrint != opalParams->end(); ++paramPrint) +// qDebug() << paramPrint.key(); + + +} + +int ParameterList::indexOf(const Parameter &p) +{ + // incase p is a copy of the actual parameter we want (i.e., addresses differ) + Parameter *pPtr = &((*params)[p.getComponentID()][p.getParamID()]); + + QList >::const_iterator iter; + int index = -1; + for (iter = paramList->begin(); iter != paramList->end(); ++iter) + { + if ((index = (*iter).indexOf(pPtr)) != -1) + return index; + } + return index; +} + + +ParameterList::const_iterator::const_iterator(QList paramList) +{ + this->paramList = QList(paramList); + index = 0; +} + +ParameterList::const_iterator::const_iterator(const const_iterator &other) +{ + paramList = QList(other.paramList); + index = other.index; +} + +ParameterList::const_iterator ParameterList::begin() const +{ + QList > compList = params->values(); + QList paramList; + QList >::const_iterator compIter; + for (compIter = compList.begin(); compIter != compList.end(); ++compIter) + paramList.append((*compIter).values()); + return const_iterator(paramList); +} + +ParameterList::const_iterator ParameterList::end() const +{ + const_iterator iter = begin(); + return iter+=iter.paramList.size(); +} + +int ParameterList::count() +{ + int count = 0; + QList >::const_iterator iter; + for (iter = paramList->begin(); iter != paramList->end(); ++iter) + count += (*iter).count(); + return count; +} + +/* Functions related to reading the xml config file */ + +bool ParameterList::open(QString filename) +{ + QFile paramFile(filename); + if (!paramFile.exists()) + { + /// \todo open dialog box (maybe: that could also go in comm config window) + return false; + } + + if (!paramFile.open(QIODevice::ReadOnly)) + { + return false; + } + + read(¶mFile); + + return true; +} + +bool ParameterList::read(QIODevice *device) +{ + QDomDocument *paramConfig = new QDomDocument(); + + QString errorStr; + int errorLine; + int errorColumn; + + if (!paramConfig->setContent(device, true, &errorStr, &errorLine, + &errorColumn)) + { + qDebug() << "Error reading XML Parameter File on line: " << errorLine << errorStr; + return false; + } + + QDomElement root = paramConfig->documentElement(); + if (root.tagName() != "ParameterList") { + qDebug() << __FILE__ << __LINE__ << "This is not a parameter list xml file"; + return false; + } + + QDomElement child = root.firstChildElement("Block"); + while (!child.isNull()) + { + parseBlock(child); + child = child.nextSiblingElement("Block"); + } + + delete paramConfig; + return true; +} + +void ParameterList::parseBlock(const QDomElement &block) +{ + + QDomNodeList paramList; + QDomElement e; + Parameter *p; + SubsystemIds id; + if (block.attribute("name") == "Navigation") + id = OpalRT::NAV_ID; + else if (block.attribute("name") == "Controller") + id = OpalRT::CONTROLLER_ID; + else if (block.attribute("name") == "ServoOutputs") + id = OpalRT::SERVO_OUTPUTS; + else if (block.attribute("name") == "ServoInputs") + id = OpalRT::SERVO_INPUTS; + + paramList = block.elementsByTagName("Parameter"); + for (int i=0; i < paramList.size(); ++i) + { + e = paramList.item(i).toElement(); + if (e.hasAttribute("SimulinkPath") && + e.hasAttribute("SimulinkParameterName") && + e.hasAttribute("QGCParamID")) + { + + p = new Parameter(e.attribute("SimulinkPath"), + e.attribute("SimulinkParameterName"), + static_cast(id), + QGCParamID(e.attribute("QGCParamID"))); + (*params)[id].insert(p->getParamID(), *p); + delete p; + } + else + { + qDebug() << __FILE__ << ":" << __LINE__ << ": error in xml doc"; + } + } + +} diff --git a/src/comm/ParameterList.h b/src/comm/ParameterList.h new file mode 100644 index 0000000000000000000000000000000000000000..f93ffac779a3a4b96f55408ab7bc655409812bae --- /dev/null +++ b/src/comm/ParameterList.h @@ -0,0 +1,142 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +#ifndef PARAMETERLIST_H +#define PARAMETERLIST_H + +#include +#include +#include +#include +#include +#include +#include + +#include "mavlink_types.h" +#include "QGCParamID.h" +#include "Parameter.h" +#include "OpalRT.h" + +namespace OpalRT +{ + class ParameterList + { + public: + + class const_iterator + { + friend class ParameterList; + + public: + inline const_iterator() {} + const_iterator(const const_iterator& other); + + const_iterator& operator+=(int i) {index += i; return *this;} + bool operator<(const const_iterator& other) {return (this->paramList == other.paramList) + &&(this->indexparamList == other.paramList) + &&(this->index==other.index);} + bool operator!=(const const_iterator& other) {return !((*this) == other);} + const Parameter& operator*() const {return paramList[index];} + const Parameter* operator->() const {return ¶mList[index];} + + const_iterator& operator++() {++index; return *this;} + private: + const_iterator(QList); + QList paramList; + int index; + }; + + + ParameterList(); + ~ParameterList(); + + /** Count the number of parameters in the list. + \return Total number of parameters + */ + int count(); + + /** Find p in the list and return its index. + \note In order to use this index to look up p, the component is also needed. + \return the index of p or -1 if p is not found + \example + \code + int compid = OpalRT::CONTROLLER_ID; + Parameter p("simulinkpath", "simulinkparamname", compid, QGCParamID("PID_GAIN")); + ParameterList pList; + if ((int index=pList.indexOf(p)) != -1) + qDebug() << "PID_GAIN is at index " << index; + \endcode + */ + int indexOf(const Parameter& p); + bool contains(int compid, QGCParamID paramid) const {return (*params)[compid].contains(paramid);} + + /// Get a parameter from the list + const Parameter getParameter(int compid, QGCParamID paramid) const {return (*params)[compid][paramid];} + Parameter& getParameter(int compid, QGCParamID paramid) {return (*params)[compid][paramid];} + const Parameter getParameter(int compid, int index) const {return *((*paramList)[compid][index]);} + + /** Convenient syntax for calling OpalRT::Parameter::getParameter() */ + Parameter& operator()(int compid, QGCParamID paramid) {return getParameter(compid, paramid);} + Parameter& operator()(uint8_t compid, QGCParamID paramid) {return getParameter(static_cast(compid), paramid);} + + const_iterator begin() const; + const_iterator end() const; + + protected: + /** Store the parameters mapped by componentid, and paramid. + \code + // Look up a parameter + int compid = 1; + QGCParamID paramid("PID_GAIN"); + Parameter p = params[compid][paramid]; + \endcode + */ + QMap > *params; + /** + Store pointers to the parameters to allow fast lookup by index. + This variable may be changed to const pointers to ensure all changes + are made through the map container. + */ + QList > *paramList; + /** + Get the list of available parameters from Opal-RT. + \param[out] opalParams Map of parameter paths/names to ids which are valid in Opal-RT + */ + void getParameterList(QMap* opalParams); + + /** + Open a file for reading in the xml config data + */ + bool open(QString filename=QString()); + /** + Attempt to read XML configuration data from device + \param[in] the device to read the xml data from + \return true if the configuration was read successfully, false otherwise + */ + bool read(QIODevice *device); + + void parseBlock(const QDomElement &block); + }; +} +#endif // PARAMETERLIST_H diff --git a/src/comm/QGCParamID.cc b/src/comm/QGCParamID.cc new file mode 100644 index 0000000000000000000000000000000000000000..804835a6540fce7ac406b42d96d11a5acb56cd3e --- /dev/null +++ b/src/comm/QGCParamID.cc @@ -0,0 +1,51 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Implementation of class OpalRT::QGCParamID + * @author Bryan Godbolt + */ + +#include "QGCParamID.h" +using namespace OpalRT; + +QGCParamID::QGCParamID(const char paramid[]):data(paramid) +{ +} + +QGCParamID::QGCParamID(const QString s):data(s) +{ + +} + +QGCParamID::QGCParamID(const QGCParamID &other):data(other.data) +{ + +} + +// +//QDataStream& operator<<(QDataStream& stream, const QGCParamID& paramid) +//{ +// return stream << paramid.data; +//} diff --git a/src/comm/QGCParamID.h b/src/comm/QGCParamID.h new file mode 100644 index 0000000000000000000000000000000000000000..90cf2fcff92727946fca1212263dfd7253fe6fde --- /dev/null +++ b/src/comm/QGCParamID.h @@ -0,0 +1,72 @@ +/*===================================================================== + +QGroundControl Open Source Ground Control Station + +(c) 2009, 2010 QGROUNDCONTROL PROJECT + +This file is part of the QGROUNDCONTROL project + + QGROUNDCONTROL is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + QGROUNDCONTROL is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with QGROUNDCONTROL. If not, see . + +======================================================================*/ + +/** + * @file + * @brief Stores the paramid used for mavlink + * @author Bryan Godbolt + */ + +#ifndef QGCPARAMID_H +#define QGCPARAMID_H + +#include + +#include "mavlink_types.h" + +//namespace OpalRT +//{ +// class QGCParamID; +//} +// +//QDataStream& operator<<(QDataStream&, const OpalRT::QGCParamID&); + +namespace OpalRT +{ + /** Stores a param_id for the mavlink parameter packets. This class adds the convenience + of storing the id as a string (e.g., easy comparison). + + \todo Fix: warning: deprecated conversion from string constant to 'char*' + */ + class QGCParamID + { +// friend QDataStream& operator<<(QDataStream& stream, const QGCParamID& paramid); + public: + + QGCParamID(const char[]); + QGCParamID(const QString); + QGCParamID() {} + QGCParamID(const QGCParamID& other); + + bool operator<(const QGCParamID& other) const {return data(data);} + int8_t* toInt8_t() const {return (int8_t*)data.toAscii().data();} + + protected: + QString data; + }; +} +#endif // QGCPARAMID_H diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 08fecb73cff3b62b76761f14bc3544a3501e1e9f..5d65c4f4f09d196a995b5c17de3c85d73604eedb 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -222,7 +222,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) // LOW BATTERY ALARM float chargeLevel = getChargeLevel(); - if (chargeLevel <= 10.0f) + if (chargeLevel <= 20.0f) { startLowBattAlarm(); } @@ -538,6 +538,21 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit textMessageReceived(uasId, message.compid, severity, text); } break; +#ifdef MAVLINK_ENABLED_UALBERTA_MESSAGES + case MAVLINK_MSG_ID_NAV_FILTER_BIAS: + { + mavlink_nav_filter_bias_t bias; + mavlink_msg_nav_filter_bias_decode(&message, &bias); + quint64 time = MG::TIME::getGroundTimeNow(); + emit valueChanged(uasId, "b_f[0]", bias.accel_0, time); + emit valueChanged(uasId, "b_f[1]", bias.accel_1, time); + emit valueChanged(uasId, "b_f[2]", bias.accel_2, time); + emit valueChanged(uasId, "b_w[0]", bias.gyro_0, time); + emit valueChanged(uasId, "b_w[1]", bias.gyro_1, time); + emit valueChanged(uasId, "b_w[2]", bias.gyro_2, time); + } + break; +#endif default: { if (!unknownPackets.contains(message.msgid)) diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 02ef012214870789727e654e9a6e058e99b430b6..38bab37e092e8a030c78401f282fcb43a4ffb780 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -451,6 +451,7 @@ void UASWaypointManager::writeWaypoints() current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER; //clear local buffer + //TODO: Why not replace with waypoint_buffer.clear() ? while(!waypoint_buffer.empty()) { delete waypoint_buffer.back(); @@ -473,6 +474,7 @@ void UASWaypointManager::writeWaypoints() cur_d->orbit_direction = 0; cur_d->param1 = cur_s->getOrbit(); cur_d->param2 = cur_s->getHoldTime(); + // TODO: Replace this value depending on the type of waypoint cur_d->type = 1; //FIXME: we only use local waypoints at the moment cur_d->seq = i; // don't read out the sequence number of the waypoint class cur_d->x = cur_s->getX(); diff --git a/src/ui/CommConfigurationWindow.cc b/src/ui/CommConfigurationWindow.cc index a51303e885c4926de4372ae72ed3d7e02c208730..5aba706ef7494e04afa19a83e73594c83be68501 100644 --- a/src/ui/CommConfigurationWindow.cc +++ b/src/ui/CommConfigurationWindow.cc @@ -43,6 +43,7 @@ This file is part of the QGROUNDCONTROL project #include "MAVLinkSimulationLink.h" #ifdef OPAL_RT #include "OpalLink.h" +#include "OpalLinkConfigurationWindow.h" #endif #include "MAVLinkProtocol.h" #include "MAVLinkSettingsWidget.h" @@ -114,6 +115,10 @@ CommConfigurationWindow::CommConfigurationWindow(LinkInterface* link, ProtocolIn OpalLink* opal = dynamic_cast(link); if (opal != 0) { + QWidget* conf = new OpalLinkConfigurationWindow(opal, this); + QBoxLayout* layout = new QBoxLayout(QBoxLayout::LeftToRight, ui.linkGroupBox); + layout->addWidget(conf); + ui.linkGroupBox->setLayout(layout); ui.linkGroupBox->setTitle(tr("Opal-RT Link")); } #endif diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index ce4160299a8a8c6b274accf034caee52f2e071fa..b38304e9f10a20cfdbe148110667758fe66f53b4 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -61,8 +61,8 @@ This file is part of the QGROUNDCONTROL project * @see QMainWindow::show() **/ MainWindow::MainWindow(QWidget *parent) : - QMainWindow(parent), - settings() + QMainWindow(parent), + settings() { this->hide(); this->setVisible(false); @@ -92,7 +92,7 @@ MainWindow::MainWindow(QWidget *parent) : // Create actions connectActions(); - // Load widgets and show application window + // Load widgets and show application windowa loadWidgets(); // Adjust the size @@ -108,131 +108,145 @@ MainWindow::~MainWindow() void MainWindow::buildWidgets() { - //FIXME: memory of acceptList will never be freed again - QStringList* acceptList = new QStringList(); - acceptList->append("roll IMU"); - acceptList->append("pitch IMU"); - acceptList->append("yaw IMU"); - acceptList->append("rollspeed IMU"); - acceptList->append("pitchspeed IMU"); - acceptList->append("yawspeed IMU"); + //FIXME: memory of acceptList will never be freed again + QStringList* acceptList = new QStringList(); + acceptList->append("roll IMU"); + acceptList->append("pitch IMU"); + acceptList->append("yaw IMU"); + acceptList->append("rollspeed IMU"); + acceptList->append("pitchspeed IMU"); + acceptList->append("yawspeed IMU"); - //FIXME: memory of acceptList2 will never be freed again - QStringList* acceptList2 = new QStringList(); - acceptList2->append("Battery"); - acceptList2->append("Pressure"); + //FIXME: memory of acceptList2 will never be freed again + QStringList* acceptList2 = new QStringList(); + acceptList2->append("Battery"); + acceptList2->append("Pressure"); - //TODO: move protocol outside UI - mavlink = new MAVLinkProtocol(); + //TODO: move protocol outside UI + mavlink = new MAVLinkProtocol(); - // Center widgets - linechartWidget = new Linecharts(this); - hudWidget = new HUD(640, 480, this); - mapWidget = new MapWidget(this); - protocolWidget = new XMLCommProtocolWidget(this); - dataplotWidget = new QGCDataPlot2D(this); + // Center widgets + linechartWidget = new Linecharts(this); + hudWidget = new HUD(640, 480, this); + mapWidget = new MapWidget(this); + protocolWidget = new XMLCommProtocolWidget(this); + dataplotWidget = new QGCDataPlot2D(this); + map3DWidget = new QWidget(this); // FIXME Lionel, insert visualizer here - // Dock widgets - controlDockWidget = new QDockWidget(tr("Control"), this); - controlDockWidget->setWidget( new UASControlWidget(this) ); + // Dock widgets + controlDockWidget = new QDockWidget(tr("Control"), this); + controlDockWidget->setWidget( new UASControlWidget(this) ); - listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); - listDockWidget->setWidget( new UASListWidget(this) ); + listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); + listDockWidget->setWidget( new UASListWidget(this) ); - waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); - waypointsDockWidget->setWidget( new WaypointList(this, NULL) ); + waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); + waypointsDockWidget->setWidget( new WaypointList(this, NULL) ); - infoDockWidget = new QDockWidget(tr("Status Details"), this); - infoDockWidget->setWidget( new UASInfoWidget(this) ); + infoDockWidget = new QDockWidget(tr("Status Details"), this); + infoDockWidget->setWidget( new UASInfoWidget(this) ); - detectionDockWidget = new QDockWidget(tr("Object Recognition"), this); - detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) ); + detectionDockWidget = new QDockWidget(tr("Object Recognition"), this); + detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) ); - debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this); - debugConsoleDockWidget->setWidget( new DebugConsole(this) ); + debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this); + debugConsoleDockWidget->setWidget( new DebugConsole(this) ); - parametersDockWidget = new QDockWidget(tr("Onboard Parameters"), this); - parametersDockWidget->setWidget( new ParameterInterface(this) ); + parametersDockWidget = new QDockWidget(tr("Onboard Parameters"), this); + parametersDockWidget->setWidget( new ParameterInterface(this) ); - watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this); - watchdogControlDockWidget->setWidget( new WatchdogControl(this) ); + watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this); + watchdogControlDockWidget->setWidget( new WatchdogControl(this) ); - hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this); - hsiDockWidget->setWidget( new HSIDisplay(this) ); + hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this); + hsiDockWidget->setWidget( new HSIDisplay(this) ); - headDown1DockWidget = new QDockWidget(tr("Primary Flight Display"), this); - headDown1DockWidget->setWidget( new HDDisplay(acceptList, this) ); + headDown1DockWidget = new QDockWidget(tr("Primary Flight Display"), this); + headDown1DockWidget->setWidget( new HDDisplay(acceptList, this) ); - headDown2DockWidget = new QDockWidget(tr("Payload Status"), this); - headDown2DockWidget->setWidget( new HDDisplay(acceptList2, this) ); + headDown2DockWidget = new QDockWidget(tr("Payload Status"), this); + headDown2DockWidget->setWidget( new HDDisplay(acceptList2, this) ); - rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); - rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); + rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); + rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); - // Dialogue widgets - //FIXME: free memory in destructor - joystick = new JoystickInput(); + // Dialogue widgets + //FIXME: free memory in destructor + joystick = new JoystickInput(); } +/** + * Connect all signals and slots of the main window widgets + */ void MainWindow::connectWidgets() { - if (linechartWidget) - { - connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), - linechartWidget, SLOT(addSystem(UASInterface*))); - connect(UASManager::instance(), SIGNAL(activeUASSet(int)), - linechartWidget, SLOT(selectSystem(int))); - connect(linechartWidget, SIGNAL(logfileWritten(QString)), - this, SLOT(loadDataView(QString))); - } - if (infoDockWidget && infoDockWidget->widget()) - { - connect(mavlink, SIGNAL(receiveLossChanged(int, float)), - infoDockWidget->widget(), SLOT(updateSendLoss(int, float))); - } + if (linechartWidget) + { + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), + linechartWidget, SLOT(addSystem(UASInterface*))); + connect(UASManager::instance(), SIGNAL(activeUASSet(int)), + linechartWidget, SLOT(selectSystem(int))); + connect(linechartWidget, SIGNAL(logfileWritten(QString)), + this, SLOT(loadDataView(QString))); + } + if (infoDockWidget && infoDockWidget->widget()) + { + connect(mavlink, SIGNAL(receiveLossChanged(int, float)), + infoDockWidget->widget(), SLOT(updateSendLoss(int, float))); + } + if (mapWidget && waypointsDockWidget->widget()) + { + // clear path create on the map + connect(waypointsDockWidget->widget(), SIGNAL(clearPathclicked()), mapWidget, SLOT(clearPath())); + // add Waypoint widget in the WaypointList widget when mouse clicked + connect(mapWidget, SIGNAL(captureMapCoordinateClick(QPointF)), waypointsDockWidget->widget(), SLOT(addWaypointMouse(QPointF))); + // it notifies that a waypoint global goes to do create + connect(mapWidget, SIGNAL(createGlobalWP(bool)), waypointsDockWidget->widget(), SLOT(setIsWPGlobal(bool))); + connect(mapWidget, SIGNAL(sendGeometryEndDrag(QPointF,int)), waypointsDockWidget->widget(), SLOT(waypointGlobalChanged(QPointF,int)) ); + } } void MainWindow::arrangeCenterStack() { - QStackedWidget *centerStack = new QStackedWidget(this); - if (!centerStack) return; + QStackedWidget *centerStack = new QStackedWidget(this); + if (!centerStack) return; - if (linechartWidget) centerStack->addWidget(linechartWidget); - if (protocolWidget) centerStack->addWidget(protocolWidget); - if (mapWidget) centerStack->addWidget(mapWidget); - if (hudWidget) centerStack->addWidget(hudWidget); - if (dataplotWidget) centerStack->addWidget(dataplotWidget); + if (linechartWidget) centerStack->addWidget(linechartWidget); + if (protocolWidget) centerStack->addWidget(protocolWidget); + if (mapWidget) centerStack->addWidget(mapWidget); + if (hudWidget) centerStack->addWidget(hudWidget); + if (dataplotWidget) centerStack->addWidget(dataplotWidget); - setCentralWidget(centerStack); + setCentralWidget(centerStack); } void MainWindow::configureWindowName() { - QList hostAddresses = QNetworkInterface::allAddresses(); - QString windowname = qApp->applicationName() + " " + qApp->applicationVersion(); - bool prevAddr = false; + QList hostAddresses = QNetworkInterface::allAddresses(); + QString windowname = qApp->applicationName() + " " + qApp->applicationVersion(); + bool prevAddr = false; - windowname.append(" (" + QHostInfo::localHostName() + ": "); + windowname.append(" (" + QHostInfo::localHostName() + ": "); - for (int i = 0; i < hostAddresses.size(); i++) - { - // Exclude loopback IPv4 and all IPv6 addresses - if (hostAddresses.at(i) != QHostAddress("127.0.0.1") && !hostAddresses.at(i).toString().contains(":")) - { - if(prevAddr) windowname.append("/"); - windowname.append(hostAddresses.at(i).toString()); - prevAddr = true; - } - } + for (int i = 0; i < hostAddresses.size(); i++) + { + // Exclude loopback IPv4 and all IPv6 addresses + if (hostAddresses.at(i) != QHostAddress("127.0.0.1") && !hostAddresses.at(i).toString().contains(":")) + { + if(prevAddr) windowname.append("/"); + windowname.append(hostAddresses.at(i).toString()); + prevAddr = true; + } + } - windowname.append(")"); + windowname.append(")"); - setWindowTitle(windowname); + setWindowTitle(windowname); #ifndef Q_WS_MAC - //qApp->setWindowIcon(QIcon(":/core/images/qtcreator_logo_128.png")); + //qApp->setWindowIcon(QIcon(":/core/images/qtcreator_logo_128.png")); #endif } @@ -341,7 +355,7 @@ void MainWindow::connectActions() connect(ui.actionPilotView, SIGNAL(triggered()), this, SLOT(loadPilotView())); connect(ui.actionEngineerView, SIGNAL(triggered()), this, SLOT(loadEngineerView())); connect(ui.actionOperatorView, SIGNAL(triggered()), this, SLOT(loadOperatorView())); - connect(ui.actionSettingsView, SIGNAL(triggered()), this, SLOT(loadSettingsView())); + connect(ui.action3DView, SIGNAL(triggered()), this, SLOT(load3DView())); connect(ui.actionShow_full_view, SIGNAL(triggered()), this, SLOT(loadAllView())); connect(ui.actionShow_MAVLink_view, SIGNAL(triggered()), this, SLOT(loadMAVLinkView())); connect(ui.actionShow_data_analysis_view, SIGNAL(triggered()), this, SLOT(loadDataView())); @@ -353,6 +367,8 @@ void MainWindow::connectActions() // Joystick configuration connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure())); + + } void MainWindow::showHelp() @@ -471,32 +487,32 @@ void MainWindow::UASCreated(UASInterface* uas) // FIXME Should be not inside the mainwindow if (debugConsoleDockWidget) { - DebugConsole *debugConsole = dynamic_cast(debugConsoleDockWidget->widget()); - if (debugConsole) - { - connect(uas, SIGNAL(textMessageReceived(int,int,int,QString)), - debugConsole, SLOT(receiveTextMessage(int,int,int,QString))); - } + DebugConsole *debugConsole = dynamic_cast(debugConsoleDockWidget->widget()); + if (debugConsole) + { + connect(uas, SIGNAL(textMessageReceived(int,int,int,QString)), + debugConsole, SLOT(receiveTextMessage(int,int,int,QString))); + } } // Health / System status indicator if (infoDockWidget) { - UASInfoWidget *infoWidget = dynamic_cast(infoDockWidget->widget()); - if (infoWidget) - { - infoWidget->addUAS(uas); - } + UASInfoWidget *infoWidget = dynamic_cast(infoDockWidget->widget()); + if (infoWidget) + { + infoWidget->addUAS(uas); + } } // UAS List if (listDockWidget) { - UASListWidget *listWidget = dynamic_cast(listDockWidget->widget()); - if (listWidget) - { - listWidget->addUAS(uas); - } + UASListWidget *listWidget = dynamic_cast(listDockWidget->widget()); + if (listWidget) + { + listWidget->addUAS(uas); + } } // Camera view @@ -530,7 +546,7 @@ void MainWindow::clearView() HDDisplay* hddWidget = dynamic_cast( headDown1DockWidget->widget() ); if (hddWidget) hddWidget->stop(); } - if (headDown2DockWidget) + if (headDown2DockWidget) { HDDisplay* hddWidget = dynamic_cast( headDown2DockWidget->widget() ); if (hddWidget) hddWidget->stop(); @@ -678,6 +694,18 @@ void MainWindow::loadPixhawkView() waypointsDockWidget->show(); } + // HORIZONTAL SITUATION INDICATOR + if (hsiDockWidget) + { + HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); + if (hsi) + { + hsi->start(); + addDockWidget(Qt::BottomDockWidgetArea, hsiDockWidget); + hsiDockWidget->show(); + } + } + // DEBUG CONSOLE if (debugConsoleDockWidget) { @@ -718,7 +746,7 @@ void MainWindow::loadDataView() void MainWindow::loadDataView(QString fileName) { clearView(); - + // DATAPLOT if (dataplotWidget) { @@ -842,36 +870,56 @@ void MainWindow::loadOperatorView() this->show(); } -void MainWindow::loadSettingsView() +void MainWindow::load3DView() { - clearView(); - - // LINE CHART - if (linechartWidget) - { - QStackedWidget *centerStack = dynamic_cast(centralWidget()); - if (centerStack) - { - linechartWidget->setActive(true); - centerStack->setCurrentWidget(linechartWidget); - } - } - - /* - // COMM XML - QDockWidget* container1 = new QDockWidget(tr("MAVLink XML to C Code Generator"), this); - container1->setWidget(protocol); - addDockWidget(Qt::LeftDockWidgetArea, container1);*/ + clearView(); - // ONBOARD PARAMETERS - if (parametersDockWidget) - { - addDockWidget(Qt::RightDockWidgetArea, parametersDockWidget); - parametersDockWidget->show(); - } + // 3D map + if (map3DWidget) + { + QStackedWidget *centerStack = dynamic_cast(centralWidget()); + if (centerStack) + { + //map3DWidget->setActive(true); + centerStack->setCurrentWidget(map3DWidget); + } + } + + // UAS CONTROL + if (controlDockWidget) + { + addDockWidget(Qt::LeftDockWidgetArea, controlDockWidget); + controlDockWidget->show(); + } + + // UAS LIST + if (listDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, listDockWidget); + listDockWidget->show(); + } + + // WAYPOINT LIST + if (waypointsDockWidget) + { + addDockWidget(Qt::BottomDockWidgetArea, waypointsDockWidget); + waypointsDockWidget->show(); + } + + // HORIZONTAL SITUATION INDICATOR + if (hsiDockWidget) + { + HSIDisplay* hsi = dynamic_cast( hsiDockWidget->widget() ); + if (hsi) + { + hsi->start(); + addDockWidget(Qt::LeftDockWidgetArea, hsiDockWidget); + hsiDockWidget->show(); + } + } - this->show(); -} + this->show(); + } void MainWindow::loadEngineerView() { @@ -1014,8 +1062,8 @@ void MainWindow::loadAllView() // OBJECT DETECTION if (detectionDockWidget) { - addDockWidget(Qt::RightDockWidgetArea, detectionDockWidget); - detectionDockWidget->show(); + addDockWidget(Qt::RightDockWidgetArea, detectionDockWidget); + detectionDockWidget->show(); } // LINE CHART diff --git a/src/ui/MainWindow.h b/src/ui/MainWindow.h index 057390bc66b5ee83671bb97268413c65d2d9d548..c96d86f220715852724e081a162e52b369cfd205 100644 --- a/src/ui/MainWindow.h +++ b/src/ui/MainWindow.h @@ -111,8 +111,8 @@ public slots: void loadEngineerView(); /** @brief Load view for operator */ void loadOperatorView(); - /** @brief Load view for general settings */ - void loadSettingsView(); + /** @brief Load 3D view */ + void load3DView(); /** @brief Load view with all widgets */ void loadAllView(); /** @brief Load MAVLink XML generator view */ @@ -160,6 +160,7 @@ protected: QPointer mapWidget; QPointer protocolWidget; QPointer dataplotWidget; + QPointer map3DWidget; // Dock widgets QPointer controlDockWidget; QPointer infoDockWidget; diff --git a/src/ui/MainWindow.ui b/src/ui/MainWindow.ui index 79dc075a4e3e797814320283f4dae98d077bc51e..7a936e1e7ed83c5d9050db2678bf617d92692843 100644 --- a/src/ui/MainWindow.ui +++ b/src/ui/MainWindow.ui @@ -22,6 +22,9 @@ 800 + + false + MGMainWindow @@ -35,7 +38,7 @@ 0 0 1000 - 22 + 23 @@ -72,7 +75,7 @@ - + @@ -110,7 +113,7 @@ - + :/images/actions/system-log-out.svg:/images/actions/system-log-out.svg @@ -122,7 +125,7 @@ - + :/images/categories/preferences-system.svg :/images/categories/preferences-system.svg:/images/categories/preferences-system.svg @@ -135,7 +138,7 @@ - + :/images/control/launch.svg :/images/control/launch.svg:/images/control/launch.svg @@ -145,7 +148,7 @@ - + :/images/control/land.svg:/images/control/land.svg @@ -170,7 +173,7 @@ - + :/images/actions/list-add.svg:/images/actions/list-add.svg @@ -179,7 +182,7 @@ - + :/images/categories/applications-system.svg:/images/categories/applications-system.svg @@ -191,7 +194,7 @@ - + :/images/apps/utilities-system-monitor.svg:/images/apps/utilities-system-monitor.svg @@ -200,7 +203,7 @@ - + :/images/status/weather-overcast.svg:/images/status/weather-overcast.svg @@ -209,7 +212,7 @@ - + :/images/categories/applications-internet.svg:/images/categories/applications-internet.svg @@ -218,7 +221,7 @@ - + :/images/devices/input-gaming.svg:/images/devices/input-gaming.svg @@ -227,7 +230,7 @@ - + :/images/status/network-wireless-encrypted.svg:/images/status/network-wireless-encrypted.svg @@ -237,13 +240,16 @@ Shop the 2D map and system status - + - + :/images/categories/preferences-system.svg:/images/categories/preferences-system.svg - Show settings view + Show 3D view + + + Show 3D view @@ -251,7 +257,7 @@ true - + :/images/control/launch.svg:/images/control/launch.svg @@ -263,7 +269,7 @@ - + :/images/status/network-transmit-receive.svg:/images/status/network-transmit-receive.svg @@ -272,7 +278,7 @@ - + :/images/devices/network-wired.svg:/images/devices/network-wired.svg @@ -281,7 +287,7 @@ - + :/images/categories/applications-internet.svg:/images/categories/applications-internet.svg @@ -290,7 +296,7 @@ - + :/images/apps/utilities-system-monitor.svg:/images/apps/utilities-system-monitor.svg @@ -299,7 +305,7 @@ - + :/images/status/software-update-available.svg:/images/status/software-update-available.svg @@ -308,7 +314,7 @@ - + :/images/categories/preferences-system.svg:/images/categories/preferences-system.svg @@ -317,9 +323,7 @@ - - - + actionExit diff --git a/src/ui/MapWidget.cc b/src/ui/MapWidget.cc index 4811d955746e7aa9e3a1ad73d48b0a959a4fe813..5673d504387c4a8b8bc50080c71fe8dd54d6b23f 100644 --- a/src/ui/MapWidget.cc +++ b/src/ui/MapWidget.cc @@ -87,7 +87,10 @@ MapWidget::MapWidget(QWidget *parent) : // Set default zoom level mc->setZoom(16); // Zurich, ETH - mc->setView(QPointF(8.548056,47.376389)); + //mc->setView(QPointF(8.548056,47.376389)); + + // Veracruz Mexico, ETH + mc->setView(QPointF(-96.105208,19.138955)); // Add controls to select map provider ///////////////////////////////////////////////// @@ -299,24 +302,32 @@ void MapWidget::createPathButtonClicked(bool checked) { Q_UNUSED(checked); + + if (createPath->isChecked()) { // change the cursor shape this->setCursor(Qt::PointingHandCursor); mc->setMouseMode(qmapcontrol::MapControl::None); - // Clear the previous WP track - // TODO: Move this to an actual clear track button and add a warning dialog - mc->layer("Waypoints")->clearGeometries(); - wps.clear(); - path->setPoints(wps); - mc->layer("Waypoints")->addGeometry(path); - wpIndex.clear(); + // emit signal start to create a Waypoint global + emit createGlobalWP(true); + +// // Clear the previous WP track +// // TODO: Move this to an actual clear track button and add a warning dialog +// mc->layer("Waypoints")->clearGeometries(); +// wps.clear(); +// path->setPoints(wps); +// mc->layer("Waypoints")->addGeometry(path); +// wpIndex.clear(); } else { + this->setCursor(Qt::ArrowCursor); mc->setMouseMode(qmapcontrol::MapControl::Panning); + + } } @@ -328,7 +339,7 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina // Create waypoint name QString str; - str = QString("WP%1").arg(path->numberOfPoints()); + str = QString("%1").arg(path->numberOfPoints()); // create the WP and set everything in the LineString to display the path CirclePoint* tempCirclePoint = new CirclePoint(coordinate.x(), coordinate.y(), 10, str); @@ -342,6 +353,10 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina // Refresh the screen mc->updateRequestNew(); + + // emit signal mouse was clicked + emit captureMapCoordinateClick(coordinate); + } } @@ -357,6 +372,10 @@ void MapWidget::captureGeometryClick(Geometry* geom, QPoint point){ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate){ Q_UNUSED(coordinate); + // Refresh the screen + mc->updateRequestNew(); + + int temp = 0; Point* point2Find; point2Find = wpIndex[geom->name()]; point2Find->setCoordinate(coordinate); @@ -364,12 +383,18 @@ void MapWidget::captureGeometryDrag(Geometry* geom, QPointF coordinate){ point2Find = dynamic_cast (geom); point2Find->setCoordinate(coordinate); - // Refresh the screen - mc->updateRequestNew(); + // qDebug() << geom->name(); + temp = geom->get_myIndex(); + //qDebug() << temp; + emit sendGeometryEndDrag(coordinate,temp); + + } -void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate){ - mc->setMouseMode(qmapcontrol::MapControl::Panning); +void MapWidget::captureGeometryEndDrag(Geometry* geom, QPointF coordinate) +{ + + mc->setMouseMode(qmapcontrol::MapControl::Panning); // qDebug() << geom->name(); // qDebug() << geom->GeometryType; @@ -536,3 +561,21 @@ void MapWidget::changeEvent(QEvent *e) break; } } +void MapWidget::clearPath() +{ + // Clear the previous WP track + + mc->layer("Waypoints")->clearGeometries(); + wps.clear(); + path->setPoints(wps); + mc->layer("Waypoints")->addGeometry(path); + wpIndex.clear(); + mc->updateRequestNew(); + + // si el boton de crear wp globales esta activo desactivarlo llamando a su evento clicket + if(createPath->isChecked()) + { + createPath->click(); + } + +} diff --git a/src/ui/MapWidget.h b/src/ui/MapWidget.h index 17929329c47bbd9a372d4dd5589b450ce968b732..7d79949d6c2f7412b764d3f2d9f01a8b48c05056 100644 --- a/src/ui/MapWidget.h +++ b/src/ui/MapWidget.h @@ -63,6 +63,9 @@ public slots: void updateGlobalPosition(UASInterface* uas, double lat, double lon, double alt, quint64 usec); void updatePosition(float time, double lat, double lon); + //ROCA + void clearPath(); + protected: void changeEvent(QEvent* e); void wheelEvent(QWheelEvent *event); @@ -108,8 +111,14 @@ protected: void captureGeometryDrag(Geometry* geom, QPointF coordinate); void captureGeometryEndDrag(Geometry* geom, QPointF coordinate); + + signals: - void movePoint(QPointF newCoord); + //void movePoint(QPointF newCoord); + void captureMapCoordinateClick(const QPointF coordinate); //ROCA + void createGlobalWP(bool value); + void sendGeometryEndDrag(const QPointF coordinate, const int index); + private: Ui::MapWidget *m_ui; diff --git a/src/ui/OpalLinkConfigurationWindow.cc b/src/ui/OpalLinkConfigurationWindow.cc new file mode 100644 index 0000000000000000000000000000000000000000..eba4328156d593d7a229c64ae2857c7dce5c8e52 --- /dev/null +++ b/src/ui/OpalLinkConfigurationWindow.cc @@ -0,0 +1,25 @@ +#include "OpalLinkConfigurationWindow.h" + +OpalLinkConfigurationWindow::OpalLinkConfigurationWindow(OpalLink* link, + QWidget *parent, + Qt::WindowFlags flags) : + QWidget(parent, flags), + link(link) + +{ + + + ui.setupUi(this); + + ui.opalInstIDSpinBox->setValue(this->link->getOpalInstID()); + + connect(ui.opalInstIDSpinBox, SIGNAL(valueChanged(int)), link, SLOT(setOpalInstID(int))); + connect(link, SIGNAL(connected(bool)), this, SLOT(allowSettingsAccess(bool))); + this->show(); +} + +void OpalLinkConfigurationWindow::allowSettingsAccess(bool enabled) +{ + ui.paramFileButton->setEnabled(enabled); + ui.servoConfigFileButton->setEnabled(enabled); +} diff --git a/src/ui/OpalLinkConfigurationWindow.h b/src/ui/OpalLinkConfigurationWindow.h new file mode 100644 index 0000000000000000000000000000000000000000..fdac559189aed2f16c0175e0b06dff18513d384f --- /dev/null +++ b/src/ui/OpalLinkConfigurationWindow.h @@ -0,0 +1,27 @@ +#ifndef OPALLINKCONFIGURATIONWINDOW_H +#define OPALLINKCONFIGURATIONWINDOW_H + +#include +#include + +#include "LinkInterface.h" +#include "ui_OpalLinkSettings.h" +#include "OpalLink.h" + +class OpalLinkConfigurationWindow : public QWidget +{ +Q_OBJECT +public: + explicit OpalLinkConfigurationWindow(OpalLink* link, QWidget *parent = 0, Qt::WindowFlags flags = Qt::Sheet); +signals: + +public slots: + + void allowSettingsAccess(bool enabled); + +private: + Ui::OpalLinkSettings ui; + OpalLink* link; +}; + +#endif // OPALLINKCONFIGURATIONWINDOW_H diff --git a/src/ui/OpalLinkSettings.ui b/src/ui/OpalLinkSettings.ui new file mode 100644 index 0000000000000000000000000000000000000000..8fd6796fb68e9073cb0da17ba018b4f23d91b709 --- /dev/null +++ b/src/ui/OpalLinkSettings.ui @@ -0,0 +1,173 @@ + + + OpalLinkSettings + + + + 0 + 0 + 537 + 250 + + + + OpalLink Configuration + + + + 6 + + + + + Model Instance ID + + + + + + + Opal-RT Parameter File + + + + + + + Servo Configuration File + + + + + + + Qt::Vertical + + + + 431 + 47 + + + + + + + + + + + + + + + + + + + + + + false + + + Change + + + + :/images/status/folder-open.svg:/images/status/folder-open.svg + + + + + + + false + + + Change + + + + :/images/status/folder-open.svg:/images/status/folder-open.svg + + + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + 101 + + + 200 + + + 101 + + + + + + + + + Delete + + + Delete this link + + + + + Connect + + + Connect this link + + + + + Close + + + Close the configuration window + + + + + + + + + actionClose + triggered() + OpalLinkSettings + close() + + + -1 + -1 + + + 224 + 195 + + + + + diff --git a/src/ui/WaypointGlobalView.cpp b/src/ui/WaypointGlobalView.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b57df7ffd96d560c8582c34c3926cea341d11bff --- /dev/null +++ b/src/ui/WaypointGlobalView.cpp @@ -0,0 +1,155 @@ +#include "WaypointGlobalView.h" +#include "ui_WaypointGlobalView.h" + +#include + +WaypointGlobalView::WaypointGlobalView(Waypoint* wp,QWidget *parent) : + QWidget(parent), + ui(new Ui::WaypointGlobalView) +{ + ui->setupUi(this); + this->wp = wp; + + ui->m_orbitalSpinBox->hide(); + + connect(ui->m_orbitalSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setOrbit(double))); + connect(ui->m_heigthSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setZ(double))); + + connect(ui->m_orbitCheckBox, SIGNAL(stateChanged(int)), this, SLOT(changeOrbitalState(int))); + + + // Read values and set user interface + updateValues(); + + +// connect(m_ui->xSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double))); +// connect(m_ui->ySpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double))); +// connect(m_ui->zSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setZ(double))); + +// //hidden degree to radian conversion of the yaw angle +// connect(m_ui->yawSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setYaw(int))); +// connect(this, SIGNAL(setYaw(double)), wp, SLOT(setYaw(double))); + +// connect(m_ui->upButton, SIGNAL(clicked()), this, SLOT(moveUp())); +// connect(m_ui->downButton, SIGNAL(clicked()), this, SLOT(moveDown())); +// connect(m_ui->removeButton, SIGNAL(clicked()), this, SLOT(remove())); + +// connect(m_ui->autoContinue, SIGNAL(stateChanged(int)), this, SLOT(changedAutoContinue(int))); +// connect(m_ui->selectedBox, SIGNAL(stateChanged(int)), this, SLOT(changedCurrent(int))); + +// +// connect(m_ui->holdTimeSpinBox, SIGNAL(valueChanged(int)), wp, SLOT(setHoldTime(int))); +} + +WaypointGlobalView::~WaypointGlobalView() +{ + delete ui; +} + +void WaypointGlobalView::updateValues() +{ + ui->m_latitudtextEdit->setText(getLatitudString(wp->getY())); + ui->m_longitudtextEdit->setText(getLongitudString(wp->getX())); + ui->idWP_label->setText(QString("%1").arg(wp->getId()));\ + +} + +void WaypointGlobalView::changeEvent(QEvent *e) +{ + switch (e->type()) { + case QEvent::LanguageChange: + ui->retranslateUi(this); + break; + default: + break; + } +} + +void WaypointGlobalView::remove() +{ + emit removeWaypoint(wp); + delete this; +} + +QString WaypointGlobalView::getLatitudString(float latitud) +{ + QString tempNS =""; + QString stringLatitudTemp = ""; + + float minutos = 0; + float grados = 0; + float entero = 0; + float dec = 0; + + if (latitud<0){tempNS="S"; latitud = latitud * -1;} + else {tempNS="N";} + + if(latitud< 90 || latitud > -90) + { + dec = latitud - (entero = ::floor(latitud));; + minutos = dec * 60; + grados = entero; + if(grados < 0) grados = grados * (-1); + if(minutos < 0) minutos = minutos * (-1); + + stringLatitudTemp = QString::number(grados)+ " ° "+ QString::number(minutos)+"' "+ tempNS; + + return stringLatitudTemp; + } + else + { + stringLatitudTemp = "erroneous latitude"; + return stringLatitudTemp; + } + +} + +QString WaypointGlobalView::getLongitudString(float longitud) +{ + QString tempEW =""; + QString stringLongitudTemp = ""; + + float minutos = 0; + float grados = 0; + float entero = 0; + float dec = 0; + + if (longitud<0){tempEW="W"; longitud = longitud * -1;} + else {tempEW="E";} + + if(longitud<180 || longitud > -180) + { + dec = longitud - (entero = ::floor(longitud));; + minutos = dec * 60; + grados = entero; + if(grados < 0) grados = grados * (-1); + if(minutos < 0) minutos = minutos * (-1); + + stringLongitudTemp = QString::number(grados)+ " ° "+ QString::number(minutos)+"' "+ tempEW; + + return stringLongitudTemp; + } + else + { + stringLongitudTemp = "erroneous longitude"; + return stringLongitudTemp; + } +} + +void WaypointGlobalView::changeOrbitalState(int state) +{ + Q_UNUSED(state); + + if(ui->m_orbitCheckBox->isChecked()) + { + ui->m_orbitalSpinBox->setEnabled(true); + ui->m_orbitalSpinBox->show(); + } + else + { + ui->m_orbitalSpinBox->setEnabled(false); + ui->m_orbitalSpinBox->hide(); + } +} + + diff --git a/src/ui/WaypointGlobalView.h b/src/ui/WaypointGlobalView.h new file mode 100644 index 0000000000000000000000000000000000000000..293b8b1228c90ab82acb318c6bc05197a2d4f0f2 --- /dev/null +++ b/src/ui/WaypointGlobalView.h @@ -0,0 +1,44 @@ +#ifndef WAYPOINTGLOBALVIEW_H +#define WAYPOINTGLOBALVIEW_H + +#include +#include "Waypoint.h" + +namespace Ui { + class WaypointGlobalView; +} + +class WaypointGlobalView : public QWidget +{ + Q_OBJECT + +public: + explicit WaypointGlobalView(Waypoint* wp, QWidget *parent = 0); + ~WaypointGlobalView(); + +public slots: + + void updateValues(void); + void remove(); + QString getLatitudString(float lat); + QString getLongitudString(float lon); + void changeOrbitalState(int state); + + +signals: + + void removeWaypoint(Waypoint*); + +protected: + virtual void changeEvent(QEvent *e); + + Waypoint* wp; + +private: + Ui::WaypointGlobalView *ui; + +private slots: + +}; + +#endif // WAYPOINTGLOBALVIEW_H diff --git a/src/ui/WaypointGlobalView.ui b/src/ui/WaypointGlobalView.ui new file mode 100644 index 0000000000000000000000000000000000000000..7ad20e38549153917c15e4201ab4feb2cb6faf6d --- /dev/null +++ b/src/ui/WaypointGlobalView.ui @@ -0,0 +1,460 @@ + + + WaypointGlobalView + + + + 0 + 0 + 869 + 60 + + + + + 0 + 0 + + + + + 50 + 0 + + + + Form + + + QWidget#colorIcon {} + +QWidget { +background-color: #252528; +color: #DDDDDF; +border-color: #EEEEEE; +background-clip: border; +} + +QCheckBox { +background-color: #252528; +color: #454545; +} + +QGroupBox { + border: 1px solid #EEEEEE; + border-radius: 5px; + padding: 0px 0px 0px 0px; +margin-top: 1ex; /* leave space at the top for the title */ + margin: 0px; +} + + QGroupBox::title { + subcontrol-origin: margin; + subcontrol-position: top center; /* position at the top center */ + margin: 0 3px 0px 3px; + padding: 0 3px 0px 0px; + font: bold 8px; + } + +QGroupBox#heartbeatIcon { + background-color: red; +} + + QDockWidget { + font: bold; + border: 1px solid #32345E; +} + +QPushButton { + font-weight: bold; + font-size: 12px; + border: 1px solid #999999; + border-radius: 10px; + min-width:22px; + max-width: 36px; + min-height: 16px; + max-height: 16px; + padding: 2px; + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #777777, stop: 1 #555555); +} + +QPushButton:pressed { + background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, stop: 0 #444444, stop: 1 #555555); +} + +QPushButton#landButton { + color: #000000; + background: qlineargradient(x1:0, y1:0, x2:0, y2:1, + stop:0 #ffee01, stop:1 #ae8f00) url("ICONDIR/control/emergency-button.png"); +} + +QPushButton:pressed#landButton { + color: #000000; + background: qlineargradient(x1:0, y1:0, x2:0, y2:1, + stop:0 #bbaa00, stop:1 #a05b00) url("ICONDIR/control/emergency-button.png"); +} + +QPushButton#killButton { + color: #000000; + background: qlineargradient(x1:0, y1:0, x2:0, y2:1, + stop:0 #ffb917, stop:1 #b37300) url("ICONDIR/control/emergency-button.png"); +} + +QPushButton:pressed#killButton { + color: #000000; + background: qlineargradient(x1:0, y1:0, x2:0, y2:1, + stop:0 #bb8500, stop:1 #903000) url("ICONDIR/control/emergency-button.png"); +} + +QProgressBar { + border: 1px solid white; + border-radius: 4px; + text-align: center; + padding: 2px; + color: white; + background-color: #111111; +} + +QProgressBar:horizontal { + height: 12px; +} + +QProgressBar QLabel { + font-size: 8px; +} + +QProgressBar:vertical { + width: 12px; +} + +QProgressBar::chunk { + background-color: #656565; +} + +QProgressBar::chunk#batteryBar { + background-color: green; +} + +QProgressBar::chunk#speedBar { + background-color: yellow; +} + +QProgressBar::chunk#thrustBar { + background-color: orange; +} + + + + + + + 0 + 0 + + + + + + + + + + WP_id + + + + + + + Qt::Horizontal + + + + 135 + 13 + + + + + + + + Lat: + + + + + + + + 0 + 0 + + + + + 120 + 25 + + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + Qt::Horizontal + + + + 50 + 30 + + + + + + + + Lon: + + + + + + + + 120 + 25 + + + + Qt::ScrollBarAlwaysOff + + + true + + + + + + + Qt::Horizontal + + + + 50 + 30 + + + + + + + + Heigth + + + + + + + + + + + + + + + 69 + 69 + 69 + + + + + + + 37 + 37 + 40 + + + + + + + 69 + 69 + 69 + + + + + + + 69 + 69 + 69 + + + + + + + 37 + 37 + 40 + + + + + + + 37 + 37 + 40 + + + + + + + + + 69 + 69 + 69 + + + + + + + 37 + 37 + 40 + + + + + + + 69 + 69 + 69 + + + + + + + 69 + 69 + 69 + + + + + + + 37 + 37 + 40 + + + + + + + 37 + 37 + 40 + + + + + + + + + 69 + 69 + 69 + + + + + + + 37 + 37 + 40 + + + + + + + 69 + 69 + 69 + + + + + + + 69 + 69 + 69 + + + + + + + 37 + 37 + 40 + + + + + + + 37 + 37 + 40 + + + + + + + + Orbital + + + + + + + true + + + + + + + + + + + diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index d8e80150914f3a99f6fa6cc56ff8d5929411cf3f..25b7171d71b6d79ea2dd8b546c057511987f120a 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -37,6 +37,8 @@ This file is part of the PIXHAWK project #include #include #include +#include "WaypointGlobalView.h" +#include WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : QWidget(parent), @@ -77,6 +79,8 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); + + // SET UAS AFTER ALL SIGNALS/SLOTS ARE CONNECTED setUAS(uas); @@ -84,6 +88,8 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : updateStatusLabel(""); this->setVisible(false); + isGlobalWP = false; + isLocalWP = false; } WaypointList::~WaypointList() @@ -162,18 +168,24 @@ void WaypointList::add() { if (uas) { - const QVector &waypoints = uas->getWaypointManager().getWaypointList(); - if (waypoints.size() > 0) - { - Waypoint *last = waypoints.at(waypoints.size()-1); - Waypoint *wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()); - uas->getWaypointManager().localAddWaypoint(wp); - } - else - { - Waypoint *wp = new Waypoint(0, 1.1, 1.1, -0.8, 0.0, true, true, 0.15, 2000); - uas->getWaypointManager().localAddWaypoint(wp); - } + + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); + if (waypoints.size() > 0) + { + Waypoint *last = waypoints.at(waypoints.size()-1); + Waypoint *wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()); + uas->getWaypointManager().localAddWaypoint(wp); + } + else + { + Waypoint *wp = new Waypoint(0, 1.1, 1.1, -0.8, 0.0, true, true, 0.15, 2000); + uas->getWaypointManager().localAddWaypoint(wp); + } + + isLocalWP = true; + + + } } @@ -181,17 +193,27 @@ void WaypointList::addCurrentPositonWaypoint() { if (uas) { - const QVector &waypoints = uas->getWaypointManager().getWaypointList(); - if (waypoints.size() > 0) + // For Global Waypoints + if(isGlobalWP) { - Waypoint *last = waypoints.at(waypoints.size()-1); - Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()); - uas->getWaypointManager().localAddWaypoint(wp); + isLocalWP = false; } else { - Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., true, true, 0.15, 2000); - uas->getWaypointManager().localAddWaypoint(wp); + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); + if (waypoints.size() > 0) + { + Waypoint *last = waypoints.at(waypoints.size()-1); + Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()); + uas->getWaypointManager().localAddWaypoint(wp); + } + else + { + Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., true, true, 0.15, 2000); + uas->getWaypointManager().localAddWaypoint(wp); + } + + isLocalWP = true; } } } @@ -240,53 +262,113 @@ void WaypointList::waypointListChanged() { const QVector &waypoints = uas->getWaypointManager().getWaypointList(); - // first remove all views of non existing waypoints - if (!wpViews.empty()) - { - QMapIterator viewIt(wpViews); - viewIt.toFront(); - while(viewIt.hasNext()) + // For Global Waypoints + if(isGlobalWP) { - viewIt.next(); - Waypoint *cur = viewIt.key(); - int i; - for (i = 0; i < waypoints.size(); i++) + isLocalWP = false; + // first remove all views of non existing waypoints + if (!wpGlobalViews.empty()) { - if (waypoints[i] == cur) + QMapIterator viewIt(wpGlobalViews); + viewIt.toFront(); + while(viewIt.hasNext()) { - break; + viewIt.next(); + Waypoint *cur = viewIt.key(); + int i; + for (i = 0; i < waypoints.size(); i++) + { + if (waypoints[i] == cur) + { + break; + } + } + if (i == waypoints.size()) + { + WaypointGlobalView* widget = wpGlobalViews.find(cur).value(); + widget->hide(); + listLayout->removeWidget(widget); + wpGlobalViews.remove(cur); + } } } - if (i == waypoints.size()) + + // then add/update the views for each waypoint in the list + for(int i = 0; i < waypoints.size(); i++) { - WaypointView* widget = wpViews.find(cur).value(); - widget->hide(); - listLayout->removeWidget(widget); - wpViews.remove(cur); - } + Waypoint *wp = waypoints[i]; + if (!wpGlobalViews.contains(wp)) + { + WaypointGlobalView* wpview = new WaypointGlobalView(wp, this); + wpGlobalViews.insert(wp, wpview); +// connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); +// connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); + connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); +// connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); +// connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); + } + WaypointGlobalView *wpgv = wpGlobalViews.value(wp); + wpgv->updateValues(); + listLayout->addWidget(wpgv); } - } - // then add/update the views for each waypoint in the list - for(int i = 0; i < waypoints.size(); i++) - { - Waypoint *wp = waypoints[i]; - if (!wpViews.contains(wp)) + } + else { - WaypointView* wpview = new WaypointView(wp, this); - wpViews.insert(wp, wpview); - connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); - connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); - connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); - connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); - connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); + // for local Waypoints + // first remove all views of non existing waypoints + if (!wpViews.empty()) + { + QMapIterator viewIt(wpViews); + viewIt.toFront(); + while(viewIt.hasNext()) + { + viewIt.next(); + Waypoint *cur = viewIt.key(); + int i; + for (i = 0; i < waypoints.size(); i++) + { + if (waypoints[i] == cur) + { + break; + } + } + if (i == waypoints.size()) + { + WaypointView* widget = wpViews.find(cur).value(); + widget->hide(); + listLayout->removeWidget(widget); + wpViews.remove(cur); + } + } + } + + // then add/update the views for each waypoint in the list + for(int i = 0; i < waypoints.size(); i++) + { + Waypoint *wp = waypoints[i]; + if (!wpViews.contains(wp)) + { + WaypointView* wpview = new WaypointView(wp, this); + wpViews.insert(wp, wpview); + connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); + connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); + connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); + connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); + connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); + } + WaypointView *wpv = wpViews.value(wp); + wpv->updateValues(); // update the values of the ui elements in the view + listLayout->addWidget(wpv); + } + } + + - WaypointView *wpv = wpViews.value(wp); - wpv->updateValues(); // update the values of the ui elements in the view - listLayout->addWidget(wpv); } - } + + } void WaypointList::moveUp(Waypoint* wp) @@ -352,3 +434,140 @@ void WaypointList::changeEvent(QEvent *e) } } + + +void WaypointList::on_clearWPListButton_clicked() +{ + + if (uas) + { + if(isGlobalWP) + { + emit clearPathclicked(); + + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); + while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++) + { + + WaypointGlobalView* widget = wpGlobalViews.find(waypoints[0]).value(); + + widget->remove(); + } + + + + isGlobalWP = false; + + + + } + else + { + + + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); + while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++) + { + + WaypointView* widget = wpViews.find(waypoints[0]).value(); + + widget->remove(); + + + } + + } + + } +} + +/** @brief Add a waypoint by mouse click over the map */ +void WaypointList::addWaypointMouse(QPointF coordinate) +{ + if (uas) + { + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); + if (waypoints.size() > 0) + { + Waypoint *last = waypoints.at(waypoints.size()-1); + Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()); + uas->getWaypointManager().localAddWaypoint(wp); + } + else + { + Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), -0.8, 0.0, true, true, 0.15, 2000); + uas->getWaypointManager().localAddWaypoint(wp); + } + } + +} + + /** @brief it notifies that a global waypoint goes to do created */ +void WaypointList::setIsWPGlobal(bool value) +{ + + + if(isLocalWP) + { + if(wpViews.size()!= 0) + { + + int ret = QMessageBox::warning(this, tr("My Application"), + tr("There are Waypoints local created.\n" + "Do you want to clear them?"), + QMessageBox::Ok | QMessageBox::Cancel); + + if(ret) + { + clearLocalWPWidget(); + isGlobalWP = value; + isLocalWP = !(value); + } + } + + + } + else + { + isGlobalWP = value; + } + +} + +/** @brief The MapWidget informs that a waypoint global was changed on the map */ + +void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP) +{ + if (uas) + { + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); + if (waypoints.size() > 0) + { + Waypoint *temp = waypoints.at(indexWP); + + temp->setX(coordinate.x()); + temp->setY(coordinate.y()); + + WaypointGlobalView* widget = wpGlobalViews.find(waypoints[indexWP]).value(); + widget->updateValues(); + } + } + + +} + +void WaypointList::clearLocalWPWidget() +{ + if (uas) + { + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); + while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++) + { + + WaypointView* widget = wpViews.find(waypoints[0]).value(); + + widget->remove(); + + } + } +} diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index 148749fcf7b8b8626d50646c22be72c77800f83e..b4d259338cd06a08f5a9839008f1ae40f4044ffe 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -41,6 +41,8 @@ This file is part of the QGROUNDCONTROL project #include "Waypoint.h" #include "UASInterface.h" #include "WaypointView.h" +#include "WaypointGlobalView.h" + namespace Ui { class WaypointList; @@ -72,6 +74,11 @@ public slots: void add(); /** @brief Add a waypoint at the current MAV position */ void addCurrentPositonWaypoint(); + /** @brief Add a waypoint by mouse click over the map */ + void addWaypointMouse(QPointF coordinate); + /** @brief it notifies that a global waypoint goes to do created */ + void setIsWPGlobal(bool value); + //Update events /** @brief sets statusLabel string */ @@ -83,25 +90,49 @@ public slots: /** @brief The waypoint manager informs that the waypoint list was changed */ void waypointListChanged(void); + /** @brief The MapWidget informs that a waypoint global was changed on the map */ + void waypointGlobalChanged(const QPointF coordinate, const int indexWP); + + void clearLocalWPWidget(); + // Waypoint operations void moveUp(Waypoint* wp); void moveDown(Waypoint* wp); void removeWaypoint(Waypoint* wp); + + + +signals: + void clearPathclicked(); + + + protected: virtual void changeEvent(QEvent *e); protected: QMap wpViews; + QMap wpGlobalViews; QVBoxLayout* listLayout; UASInterface* uas; double mavX; double mavY; double mavZ; double mavYaw; + bool isGlobalWP; + bool isLocalWP; private: Ui::WaypointList *m_ui; + + + + + +private slots: + void on_clearWPListButton_clicked(); + }; #endif // WAYPOINTLIST_H diff --git a/src/ui/WaypointList.ui b/src/ui/WaypointList.ui index 8ada605ccf5e8502e3c28406817897b4e932a231..587a8c929dea70c5c6745fbb82d424f6e66025a9 100644 --- a/src/ui/WaypointList.ui +++ b/src/ui/WaypointList.ui @@ -26,7 +26,7 @@ 4 - + true @@ -37,7 +37,7 @@ 0 0 466 - 149 + 163 @@ -54,7 +54,7 @@ - + Read @@ -65,7 +65,7 @@ - + Write @@ -76,7 +76,7 @@ - + TextLabel @@ -135,6 +135,17 @@ + + + + + + + + :/images/actions/process-stop.svg:/images/actions/process-stop.svg + + + diff --git a/src/ui/WaypointView.ui b/src/ui/WaypointView.ui index d7e3375bbcf9ee1e67356d723eab33ac763565da..5c8f32af7721b6f5ed1093a812acf4c8f48ec843 100644 --- a/src/ui/WaypointView.ui +++ b/src/ui/WaypointView.ui @@ -355,7 +355,7 @@ QProgressBar::chunk#thrustBar { - + :/images/actions/go-up.svg:/images/actions/go-up.svg @@ -375,7 +375,7 @@ QProgressBar::chunk#thrustBar { - + :/images/actions/go-down.svg:/images/actions/go-down.svg @@ -395,7 +395,7 @@ QProgressBar::chunk#thrustBar { - + :/images/actions/list-remove.svg:/images/actions/list-remove.svg @@ -405,8 +405,6 @@ QProgressBar::chunk#thrustBar { - - - + diff --git a/src/ui/linechart/LinechartPlot.h b/src/ui/linechart/LinechartPlot.h index 835c8def07279d24a73921b35b6f6eb2b53f2c7f..dfa2b9c5af63fc2f5e3c75b2b2210652ea1ccd23 100644 --- a/src/ui/linechart/LinechartPlot.h +++ b/src/ui/linechart/LinechartPlot.h @@ -107,7 +107,7 @@ class TimeSeriesData { public: - TimeSeriesData(QwtPlot* plot, QString friendlyName = "data", quint64 plotInterval = 30000, quint64 maxInterval = 0, double zeroValue = 0); + TimeSeriesData(QwtPlot* plot, QString friendlyName = "data", quint64 plotInterval = 10000, quint64 maxInterval = 0, double zeroValue = 0); ~TimeSeriesData(); void append(quint64 ms, double value); diff --git a/testlog2.txt b/testlog2.txt index 4bf04c5f783eb23b97a6ce3ea0c488f9436c3354..05faada916c1e6f342b5fe6aa60f4550e26d7e94 100644 --- a/testlog2.txt +++ b/testlog2.txt @@ -1,59 +1,59 @@ -unix_timestamp Accel._X Accel._Y Accel._Z Battery Bottom_Rotor CPU_Load Ground_Dist. Gyro_Phi Gyro_Psi Gyro_Theta Left_Servo Mag._X Mag._Y Mag._Z Pressure Right_Servo Temperature Top_Rotor pitch_IMU roll_IMU yaw_IMU -1270125571544 5 -197 982 0 29685 29866 29888 0 0 0 96186 354 -0.112812 0.13585 -1.94192 -1270125571564 6 -198 983 0 29781 29995 29871 0 0 0 96183 354 -0.112085 0.137193 -1.94139 -1270125571584 10 -197 982 0 29739 30018 29867 0 0 0 96171 354 -0.111693 0.136574 -1.94353 -1270125571604 7 -198 982 0 29802 29911 29803 0 0 0 96177 354 -0.112179 0.135778 -1.94617 -1270125571645 4 -197 984 0 29782 29844 29844 0 0 0 96180 354 -0.113056 0.135965 -1.94715 -1270125571665 4 -200 985 0 29685 29867 29884 0 0 0 96183 354 -0.113231 0.135327 -1.94586 -1270125571685 7 -198 983 0 29757 30018 29829 0 0 0 96177 354 -0.112604 0.136857 -1.94508 -1270125571706 6 -198 982 0 29759 29992 29867 0 0 0 96183 354 -0.1126 0.136602 -1.94915 -1270125571746 7 -196 985 0 29803 29910 29800 0 0 0 96183 354 -0.112392 0.136709 -1.95079 -1270125571766 4 -198 981 0 29750 29828 29888 0 0 0 96186 354 -0.113342 0.135532 -1.95064 -1270125571787 5 -197 984 0 29759 29931 29881 0 0 0 96174 354 -0.112628 0.135657 -1.9488 -1270125571807 10 -197 982 0 29739 29958 29819 0 0 0 96174 354 -0.112506 0.135777 -1.95001 -1270125571847 4 -198 981 0 29786 29829 29864 0 0 0 96232 356 -0.112359 0.135253 -1.94825 -1270125571867 5 -201 978 0 29719 29856 29890 0 0 0 96218 356 -0.11218 0.134669 -1.94657 -1270125571888 7 -198 986 0 29782 29813 29845 0 0 0 96223 356 -0.111451 0.135688 -1.9455 -1270125571908 10 -200 984 0 29693 30048 29885 0 0 0 96215 356 -0.110745 0.1365 -1.94668 -1270125571948 5 -197 982 0 29757 29899 29844 0 0 0 96229 356 -0.110004 0.135859 -1.94573 -1270125571969 6 -197 983 0 29727 29829 29814 0 0 0 96249 356 -0.110316 0.135751 -1.94565 -1270125571989 6 -197 978 0 29790 29828 29815 0 0 0 96220 356 -0.111159 0.136244 -1.94416 -1270125572009 5 -194 983 0 29718 29856 29890 0 0 0 96229 356 -0.111216 0.135992 -1.94196 -1270125572050 3 -196 985 0 29739 29978 29827 0 0 0 96223 356 -0.110727 0.136577 -1.94402 -1270125572070 8 -198 982 0 29782 29829 29815 0 0 0 96235 356 -0.111423 0.136697 -1.94566 -1270125572090 8 -195 982 0 29687 29865 29894 0 0 0 96218 356 -0.112088 0.136097 -1.9442 -1270125572130 8 -198 986 0 29793 29859 29844 0 0 0 96244 356 -0.110282 0.137491 -1.94235 -1270125572171 6 -198 982 0 29790 29856 29879 0 0 0 96244 356 -0.112179 0.136971 -1.94365 -1270125572191 3 -198 980 0 29759 30018 29829 0 0 0 96218 356 -0.111623 0.136194 -1.94286 -1270125572211 7 -198 984 0 29758 29829 29846 0 0 0 96229 356 -0.112275 0.136007 -1.94387 -1270125572252 8 -197 980 0 29743 30018 29824 0 0 0 96215 356 -0.111515 0.135539 -1.94281 -1270125572272 11 -195 984 0 29741 29867 29844 0 0 0 96249 356 -0.111995 0.135753 -1.94527 -1270125572292 4 -200 984 0 29771 29980 29865 0 0 0 96215 356 -0.111913 0.135829 -1.94455 -1270125572312 6 -198 983 0 29792 29837 29845 0 0 0 96229 356 -0.111884 0.134861 -1.94478 -1270125572353 5 -196 983 0 29685 29867 29884 0 0 0 96238 356 -0.111332 0.135644 -1.94784 -1270125572373 5 -198 982 0 29693 29852 29904 0 0 0 96238 356 -0.110772 0.136927 -1.94698 -1270125572393 6 -195 982 0 29738 29791 29818 0 0 0 96229 356 -0.109851 0.138106 -1.94592 -1270125572414 8 -198 986 0 29782 29844 29829 0 0 0 96223 356 -0.111077 0.137396 -1.9428 -1270125572454 11 -198 983 0 29743 29986 29833 0 0 0 96220 356 -0.110257 0.136744 -1.94155 -1270125572474 6 -198 983 0 29749 29866 29813 0 0 0 96223 356 -0.110497 0.136917 -1.94351 -1270125572495 6 -197 984 0 29719 29828 29890 0 0 0 96223 356 -0.111349 0.136959 -1.94295 -1270125572515 7 -197 983 0 29760 29791 29818 0 0 0 96232 356 -0.111323 0.137251 -1.93972 -1270125572555 10 -198 984 0 29800 30016 29857 0 0 0 96241 356 -0.110873 0.137041 -1.94196 -1270125572575 5 -200 982 0 29760 29962 29866 0 0 0 96232 356 -0.110648 0.136038 -1.9445 -1270125572596 4 -197 983 0 29740 30024 29867 0 0 0 96238 356 -0.110486 0.136046 -1.94576 -1270125572616 8 -196 985 0 29802 29783 29808 0 0 0 96235 356 -0.111227 0.135128 -1.94567 -1270125572656 5 -198 986 0 29771 29972 29867 0 0 0 96229 356 -0.111182 0.133833 -1.94823 -1270125572697 8 -196 980 0 29739 29978 29803 0 0 0 96238 356 -0.111457 0.133602 -1.94682 -1270125572717 4 -196 983 0 29749 30026 29867 0 0 0 96235 356 -0.116038 0.131922 -1.93861 -1270125592944 5 -195 985 0 29757 29994 29871 0 0 0 96220 356 -0.115848 0.1323 -1.93916 -1270125593025 5 -200 986 0 29780 29844 29845 0 0 0 96235 356 -0.115273 0.132222 -1.93576 -1270125593045 5 -197 982 0 29757 29995 29871 0 0 0 96229 356 -0.114713 0.132137 -1.93723 -1270125593086 4 -197 984 0 29802 29847 29819 0 0 0 96241 356 -0.114815 0.131529 -1.93626 -1270125593106 6 -202 984 0 29802 29783 29803 0 0 0 96247 356 -0.115379 0.130754 -1.93546 -1270125593126 8 -196 982 0 29803 29918 29793 0 0 0 96229 356 -0.116136 0.130336 -1.93339 -1270125593146 6 -196 987 0 29802 29825 29802 0 0 0 96244 356 -0.117635 0.128822 -1.93292 -1270125593187 7 -197 978 0 29802 29825 29864 0 0 0 96238 356 -0.117592 0.128252 -1.93468 -1270125593207 7 -197 980 0 29803 29918 29771 0 0 0 96235 356 -0.116961 0.127734 -1.93337 -1270125593227 6 -200 982 0 29733 29994 29867 0 0 0 96241 356 -0.11821 0.127208 -1.93399 -1270125593247 7 -194 984 0 29739 29982 29888 0 0 0 96235 356 -0.116428 0.128791 -1.9378 +unix_timestamp Accel._X Accel._Y Accel._Z Battery Bottom_Rotor CPU_Load Ground_Dist. Gyro_Phi Gyro_Psi Gyro_Theta Left_Servo Mag._X Mag._Y Mag._Z Pressure Right_Servo Temperature Top_Rotor pitch_IMU roll_IMU yaw_IMU +1270125571544 5 -197 982 0 29685 29866 29888 0 0 0 96186 354 -0.112812 0.13585 -1.94192 +1270125571564 6 -198 983 0 29781 29995 29871 0 0 0 96183 354 -0.112085 0.137193 -1.94139 +1270125571584 10 -197 982 0 29739 30018 29867 0 0 0 96171 354 -0.111693 0.136574 -1.94353 +1270125571604 7 -198 982 0 29802 29911 29803 0 0 0 96177 354 -0.112179 0.135778 -1.94617 +1270125571645 4 -197 984 0 29782 29844 29844 0 0 0 96180 354 -0.113056 0.135965 -1.94715 +1270125571665 4 -200 985 0 29685 29867 29884 0 0 0 96183 354 -0.113231 0.135327 -1.94586 +1270125571685 7 -198 983 0 29757 30018 29829 0 0 0 96177 354 -0.112604 0.136857 -1.94508 +1270125571706 6 -198 982 0 29759 29992 29867 0 0 0 96183 354 -0.1126 0.136602 -1.94915 +1270125571746 7 -196 985 0 29803 29910 29800 0 0 0 96183 354 -0.112392 0.136709 -1.95079 +1270125571766 4 -198 981 0 29750 29828 29888 0 0 0 96186 354 -0.113342 0.135532 -1.95064 +1270125571787 5 -197 984 0 29759 29931 29881 0 0 0 96174 354 -0.112628 0.135657 -1.9488 +1270125571807 10 -197 982 0 29739 29958 29819 0 0 0 96174 354 -0.112506 0.135777 -1.95001 +1270125571847 4 -198 981 0 29786 29829 29864 0 0 0 96232 356 -0.112359 0.135253 -1.94825 +1270125571867 5 -201 978 0 29719 29856 29890 0 0 0 96218 356 -0.11218 0.134669 -1.94657 +1270125571888 7 -198 986 0 29782 29813 29845 0 0 0 96223 356 -0.111451 0.135688 -1.9455 +1270125571908 10 -200 984 0 29693 30048 29885 0 0 0 96215 356 -0.110745 0.1365 -1.94668 +1270125571948 5 -197 982 0 29757 29899 29844 0 0 0 96229 356 -0.110004 0.135859 -1.94573 +1270125571969 6 -197 983 0 29727 29829 29814 0 0 0 96249 356 -0.110316 0.135751 -1.94565 +1270125571989 6 -197 978 0 29790 29828 29815 0 0 0 96220 356 -0.111159 0.136244 -1.94416 +1270125572009 5 -194 983 0 29718 29856 29890 0 0 0 96229 356 -0.111216 0.135992 -1.94196 +1270125572050 3 -196 985 0 29739 29978 29827 0 0 0 96223 356 -0.110727 0.136577 -1.94402 +1270125572070 8 -198 982 0 29782 29829 29815 0 0 0 96235 356 -0.111423 0.136697 -1.94566 +1270125572090 8 -195 982 0 29687 29865 29894 0 0 0 96218 356 -0.112088 0.136097 -1.9442 +1270125572130 8 -198 986 0 29793 29859 29844 0 0 0 96244 356 -0.110282 0.137491 -1.94235 +1270125572171 6 -198 982 0 29790 29856 29879 0 0 0 96244 356 -0.112179 0.136971 -1.94365 +1270125572191 3 -198 980 0 29759 30018 29829 0 0 0 96218 356 -0.111623 0.136194 -1.94286 +1270125572211 7 -198 984 0 29758 29829 29846 0 0 0 96229 356 -0.112275 0.136007 -1.94387 +1270125572252 8 -197 980 0 29743 30018 29824 0 0 0 96215 356 -0.111515 0.135539 -1.94281 +1270125572272 11 -195 984 0 29741 29867 29844 0 0 0 96249 356 -0.111995 0.135753 -1.94527 +1270125572292 4 -200 984 0 29771 29980 29865 0 0 0 96215 356 -0.111913 0.135829 -1.94455 +1270125572312 6 -198 983 0 29792 29837 29845 0 0 0 96229 356 -0.111884 0.134861 -1.94478 +1270125572353 5 -196 983 0 29685 29867 29884 0 0 0 96238 356 -0.111332 0.135644 -1.94784 +1270125572373 5 -198 982 0 29693 29852 29904 0 0 0 96238 356 -0.110772 0.136927 -1.94698 +1270125572393 6 -195 982 0 29738 29791 29818 0 0 0 96229 356 -0.109851 0.138106 -1.94592 +1270125572414 8 -198 986 0 29782 29844 29829 0 0 0 96223 356 -0.111077 0.137396 -1.9428 +1270125572454 11 -198 983 0 29743 29986 29833 0 0 0 96220 356 -0.110257 0.136744 -1.94155 +1270125572474 6 -198 983 0 29749 29866 29813 0 0 0 96223 356 -0.110497 0.136917 -1.94351 +1270125572495 6 -197 984 0 29719 29828 29890 0 0 0 96223 356 -0.111349 0.136959 -1.94295 +1270125572515 7 -197 983 0 29760 29791 29818 0 0 0 96232 356 -0.111323 0.137251 -1.93972 +1270125572555 10 -198 984 0 29800 30016 29857 0 0 0 96241 356 -0.110873 0.137041 -1.94196 +1270125572575 5 -200 982 0 29760 29962 29866 0 0 0 96232 356 -0.110648 0.136038 -1.9445 +1270125572596 4 -197 983 0 29740 30024 29867 0 0 0 96238 356 -0.110486 0.136046 -1.94576 +1270125572616 8 -196 985 0 29802 29783 29808 0 0 0 96235 356 -0.111227 0.135128 -1.94567 +1270125572656 5 -198 986 0 29771 29972 29867 0 0 0 96229 356 -0.111182 0.133833 -1.94823 +1270125572697 8 -196 980 0 29739 29978 29803 0 0 0 96238 356 -0.111457 0.133602 -1.94682 +1270125572717 4 -196 983 0 29749 30026 29867 0 0 0 96235 356 -0.116038 0.131922 -1.93861 +1270125592944 5 -195 985 0 29757 29994 29871 0 0 0 96220 356 -0.115848 0.1323 -1.93916 +1270125593025 5 -200 986 0 29780 29844 29845 0 0 0 96235 356 -0.115273 0.132222 -1.93576 +1270125593045 5 -197 982 0 29757 29995 29871 0 0 0 96229 356 -0.114713 0.132137 -1.93723 +1270125593086 4 -197 984 0 29802 29847 29819 0 0 0 96241 356 -0.114815 0.131529 -1.93626 +1270125593106 6 -202 984 0 29802 29783 29803 0 0 0 96247 356 -0.115379 0.130754 -1.93546 +1270125593126 8 -196 982 0 29803 29918 29793 0 0 0 96229 356 -0.116136 0.130336 -1.93339 +1270125593146 6 -196 987 0 29802 29825 29802 0 0 0 96244 356 -0.117635 0.128822 -1.93292 +1270125593187 7 -197 978 0 29802 29825 29864 0 0 0 96238 356 -0.117592 0.128252 -1.93468 +1270125593207 7 -197 980 0 29803 29918 29771 0 0 0 96235 356 -0.116961 0.127734 -1.93337 +1270125593227 6 -200 982 0 29733 29994 29867 0 0 0 96241 356 -0.11821 0.127208 -1.93399 +1270125593247 7 -194 984 0 29739 29982 29888 0 0 0 96235 356 -0.116428 0.128791 -1.9378 1270125593288 10 -197 985 0 29771 30000 29865 0 0 0 96238 356 -0.116243 0.128666 -1.93672 \ No newline at end of file