diff --git a/.gitignore b/.gitignore index 03c213b1f2285a7bc777c77d60cbc51094575be1..77e8963d9c1cb86b18054799d4ef1809ab7d530a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ *Makefile* +tags build Info.plist obj diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 6fe6281bb6d89817fb278e5009e4884d416f877c..725a6e94e397d0bf397e9c7ebdc677b957fb3f9d 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -134,8 +134,8 @@ FORMS += src/ui/MainWindow.ui \ src/ui/QGCFirmwareUpdate.ui \ src/ui/QGCPxImuFirmwareUpdate.ui \ src/ui/QGCDataPlot2D.ui \ - src/ui/QGCRemoteControlView.ui \ - src/ui/WaypointGlobalView.ui + src/ui/QGCRemoteControlView.ui + #src/ui/WaypointGlobalView.ui INCLUDEPATH += src \ src/ui \ src/ui/linechart \ @@ -202,7 +202,7 @@ HEADERS += src/MG.h \ src/ui/linechart/Linecharts.h \ src/uas/SlugsMAV.h \ src/uas/PxQuadMAV.h \ - src/uas/ArduPilotMAV.h \ + src/uas/ArduPilotMegaMAV.h \ src/comm/MAVLinkSyntaxHighlighter.h \ src/ui/watchdog/WatchdogControl.h \ src/ui/watchdog/WatchdogProcessView.h \ @@ -218,7 +218,7 @@ HEADERS += src/MG.h \ src/ui/map/Waypoint2DIcon.h \ src/ui/map/MAV2DIcon.h \ src/ui/QGCRemoteControlView.h \ - src/ui/WaypointGlobalView.h \ + #src/ui/WaypointGlobalView.h \ src/ui/RadioCalibration/RadioCalibrationData.h \ src/ui/RadioCalibration/RadioCalibrationWindow.h \ src/ui/RadioCalibration/AirfoilServoCalibrator.h \ @@ -282,7 +282,7 @@ SOURCES += src/main.cc \ src/ui/linechart/Linecharts.cc \ src/uas/SlugsMAV.cc \ src/uas/PxQuadMAV.cc \ - src/uas/ArduPilotMAV.cc \ + src/uas/ArduPilotMegaMAV.cc \ src/comm/MAVLinkSyntaxHighlighter.cc \ src/ui/watchdog/WatchdogControl.cc \ src/ui/watchdog/WatchdogProcessView.cc \ @@ -304,7 +304,7 @@ SOURCES += src/main.cc \ src/ui/RadioCalibration/CurveCalibrator.cc \ src/ui/RadioCalibration/AbstractCalibrator.cc \ src/ui/RadioCalibration/RadioCalibrationData.cc \ - src/ui/WaypointGlobalView.cc \ + #src/ui/WaypointGlobalView.cc \ src/ui/map3D/Q3DWidget.cc \ src/ui/map3D/CheetahModel.cc \ src/ui/map3D/CheetahGL.cc \ diff --git a/src/Waypoint.cc b/src/Waypoint.cc index 07a4f6f2b6d487eecab4ac1569c7110a29d0a47f..d84b76ecfa7636af9fc63acb6408ace5094a2d91 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -33,7 +33,8 @@ This file is part of the QGROUNDCONTROL project #include "Waypoint.h" #include -Waypoint::Waypoint(quint16 _id, float _x, float _y, float _z, float _yaw, bool _autocontinue, bool _current, float _orbit, int _holdTime) +Waypoint::Waypoint(quint16 _id, float _x, float _y, float _z, float _yaw, bool _autocontinue, bool _current, float _orbit, int _holdTime, + MAV_FRAME _frame, MAV_ACTION _action) : id(_id), x(_x), y(_y), @@ -42,7 +43,9 @@ Waypoint::Waypoint(quint16 _id, float _x, float _y, float _z, float _yaw, bool _ autocontinue(_autocontinue), current(_current), orbit(_orbit), - holdTime(_holdTime) + holdTime(_holdTime), + frame(_frame), + action(_action) { } @@ -100,9 +103,14 @@ void Waypoint::setYaw(float yaw) this->yaw = yaw; } -void Waypoint::setType(type_enum type) +void Waypoint::setAction(MAV_ACTION action) { - this->type = type; + this->action = action; +} + +void Waypoint::setFrame(MAV_FRAME frame) +{ + this->frame = frame; } void Waypoint::setAutocontinue(bool autoContinue) diff --git a/src/Waypoint.h b/src/Waypoint.h index 777e78488ed7478f42d88bfe44cfc76cd3bfe0af..216b60abc8a5013972bbbb7e6e1d6f07b24804b7 100644 --- a/src/Waypoint.h +++ b/src/Waypoint.h @@ -36,14 +36,16 @@ This file is part of the PIXHAWK project #include #include #include -#include +#include "mavlink_types.h" class Waypoint : public QObject { Q_OBJECT public: - Waypoint(quint16 id = 0, float x = 0.0f, float y = 0.0f, float z = 0.0f, float yaw = 0.0f, bool autocontinue = false, bool current = false, float orbit = 0.1f, int holdTime = 2000); + Waypoint(quint16 id = 0, float x = 0.0f, float y = 0.0f, float z = 0.0f, float yaw = 0.0f, bool autocontinue = false, + bool current = false, float orbit = 0.1f, int holdTime = 2000, + MAV_FRAME frame=MAV_FRAME_GLOBAL, MAV_ACTION action=MAV_ACTION_NAVIGATE); ~Waypoint(); quint16 getId() const { return id; } @@ -55,26 +57,20 @@ public: bool getCurrent() const { return current; } float getOrbit() const { return orbit; } float getHoldTime() const { return holdTime; } + MAV_FRAME getFrame() const { return frame; } + MAV_ACTION getAction() const { return action; } void save(QTextStream &saveStream); bool load(QTextStream &loadStream); - enum type_enum - { - GLOBAL = 0, - LOCAL = 1, - GLOBAL_ORBIT = 2, - LOCAL_ORBIT = 3 - }; - - protected: quint16 id; float x; float y; float z; float yaw; - type_enum type; + MAV_FRAME frame; + MAV_ACTION action; bool autocontinue; bool current; float orbit; @@ -86,7 +82,8 @@ public slots: void setY(float y); void setZ(float z); void setYaw(float yaw); - void setType(type_enum type); + void setAction(MAV_ACTION action); + void setFrame(MAV_FRAME frame); void setAutocontinue(bool autoContinue); void setCurrent(bool current); void setOrbit(float orbit); diff --git a/src/comm/MAVLinkLightProtocol.cc b/src/comm/MAVLinkLightProtocol.cc index 4ce662538bf779f3f9a272cbd7ac5c737e4f8afd..43de4e4688e659f0f6c50237d824ca3961000a45 100644 --- a/src/comm/MAVLinkLightProtocol.cc +++ b/src/comm/MAVLinkLightProtocol.cc @@ -29,7 +29,7 @@ This file is part of the QGROUNDCONTROL project #include "MAVLinkLightProtocol.h" #include "UASManager.h" -#include "ArduPilotMAV.h" +#include "ArduPilotMegaMAV.h" #include "LinkManager.h" MAVLinkLightProtocol::MAVLinkLightProtocol() : @@ -113,7 +113,7 @@ void MAVLinkLightProtocol::receiveBytes(LinkInterface* link, QByteArray b) // // Check and (if necessary) create UAS object // if (uas == NULL) // { -// ArduPilotMAV* mav = new ArduPilotMAV(this, sysid); // FIXME change to msg.sysid if this field exists +// ArduPilotMegaMAV* mav = new ArduPilotMAV(this, sysid); // FIXME change to msg.sysid if this field exists // // Connect this robot to the UAS object // // it is IMPORTANT here to use the right object type, // // else the slot of the parent object is called (and thus the special diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 5cedb4d88cd31ebfa0a10865ad6c583094eaf56b..741c2bb7b51dcecef51fc82663e29aa3f365fe61 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -42,7 +42,7 @@ This file is part of the QGROUNDCONTROL project #include "UAS.h" #include "SlugsMAV.h" #include "PxQuadMAV.h" -#include "ArduPilotMAV.h" +#include "ArduPilotMegaMAV.h" #include "configuration.h" #include "LinkManager.h" #include @@ -186,9 +186,9 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) uas = mav; } break; - case MAV_AUTOPILOT_ARDUPILOT: + case MAV_AUTOPILOT_ARDUPILOTMEGA: { - ArduPilotMAV* mav = new ArduPilotMAV(this, message.sysid); + ArduPilotMegaMAV* mav = new ArduPilotMegaMAV(this, message.sysid); // Connect this robot to the UAS object // it is IMPORTANT here to use the right object type, // else the slot of the parent object is called (and thus the special diff --git a/src/uas/ArduPilotMAV.cc b/src/uas/ArduPilotMegaMAV.cc similarity index 91% rename from src/uas/ArduPilotMAV.cc rename to src/uas/ArduPilotMegaMAV.cc index a1cbe3f1f6af0a1ff9f994f71f6459c3f35ebd4c..e43003ebc7e8ef463938c659e3052328adb75a16 100644 --- a/src/uas/ArduPilotMAV.cc +++ b/src/uas/ArduPilotMegaMAV.cc @@ -27,9 +27,9 @@ This file is part of the QGROUNDCONTROL project * @author Your Name here */ -#include "ArduPilotMAV.h" +#include "ArduPilotMegaMAV.h" -ArduPilotMAV::ArduPilotMAV(MAVLinkProtocol* mavlink, int id) : +ArduPilotMegaMAV::ArduPilotMegaMAV(MAVLinkProtocol* mavlink, int id) : UAS(mavlink, id)//, // place other initializers here { @@ -43,7 +43,7 @@ ArduPilotMAV::ArduPilotMAV(MAVLinkProtocol* mavlink, int id) : * messages can be sent back to the system via this link * @param message MAVLink message, as received from the MAVLink protocol stack */ -void ArduPilotMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) +void ArduPilotMegaMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) { // Let UAS handle the default message set UAS::receiveMessage(link, message); diff --git a/src/uas/ArduPilotMAV.h b/src/uas/ArduPilotMegaMAV.h similarity index 88% rename from src/uas/ArduPilotMAV.h rename to src/uas/ArduPilotMegaMAV.h index 29f4a183dfa585648d33ffa4be867daf3a560e19..00866830defc5fd9f9d559670f609ebb95151e0e 100644 --- a/src/uas/ArduPilotMAV.h +++ b/src/uas/ArduPilotMegaMAV.h @@ -21,16 +21,16 @@ This file is part of the QGROUNDCONTROL project ======================================================================*/ -#ifndef ARDUPILOTMAV_H -#define ARDUPILOTMAV_H +#ifndef ARDUPILOTMEGAMAV_H +#define ARDUPILOTMEGAMAV_H #include "UAS.h" -class ArduPilotMAV : public UAS +class ArduPilotMegaMAV : public UAS { Q_OBJECT public: - ArduPilotMAV(MAVLinkProtocol* mavlink, int id = 0); + ArduPilotMegaMAV(MAVLinkProtocol* mavlink, int id = 0); public slots: /** @brief Receive a MAVLink message from this MAV */ void receiveMessage(LinkInterface* link, mavlink_message_t message); diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 38bab37e092e8a030c78401f282fcb43a4ffb780..9f64729f9b7867555b04c6f095a363442b1e6f04 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -132,7 +132,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ if(wp->seq == current_wp_id) { Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2); - localAddWaypoint(lwp); + addWaypoint(lwp); //get next waypoint current_wp_id++; @@ -288,7 +288,7 @@ int UASWaypointManager::setCurrentWaypoint(quint16 seq) return -1; } -void UASWaypointManager::localAddWaypoint(Waypoint *wp) +void UASWaypointManager::addWaypoint(Waypoint *wp) { if (wp) { @@ -298,7 +298,7 @@ void UASWaypointManager::localAddWaypoint(Waypoint *wp) } } -int UASWaypointManager::localRemoveWaypoint(quint16 seq) +int UASWaypointManager::removeWaypoint(quint16 seq) { if (seq < waypoints.size()) { @@ -316,7 +316,7 @@ int UASWaypointManager::localRemoveWaypoint(quint16 seq) return -1; } -void UASWaypointManager::localMoveWaypoint(quint16 cur_seq, quint16 new_seq) +void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq) { if (cur_seq != new_seq && cur_seq < waypoints.size() && new_seq < waypoints.size()) { @@ -344,7 +344,7 @@ void UASWaypointManager::localMoveWaypoint(quint16 cur_seq, quint16 new_seq) } } -void UASWaypointManager::localSaveWaypoints(const QString &saveFile) +void UASWaypointManager::saveWaypoints(const QString &saveFile) { QFile file(saveFile); if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) @@ -358,7 +358,7 @@ void UASWaypointManager::localSaveWaypoints(const QString &saveFile) file.close(); } -void UASWaypointManager::localLoadWaypoints(const QString &loadFile) +void UASWaypointManager::loadWaypoints(const QString &loadFile) { QFile file(loadFile); if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) @@ -386,16 +386,6 @@ void UASWaypointManager::localLoadWaypoints(const QString &loadFile) emit waypointListChanged(); } -void UASWaypointManager::globalAddWaypoint(Waypoint *wp) -{ - -} - -int UASWaypointManager::globalRemoveWaypoint(quint16 seq) -{ - return 0; -} - void UASWaypointManager::clearWaypointList() { if(current_state == WP_IDLE) @@ -474,8 +464,8 @@ 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->frame = cur_s->getFrame(); + cur_d->action = cur_s->getAction(); cur_d->seq = i; // don't read out the sequence number of the waypoint class cur_d->x = cur_s->getX(); cur_d->y = cur_s->getY(); diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index a16c8d3fe9a433a2af487b62f1da8751f042ab7d..85bd61421ce5d229df76a10496dec5c2a7642e7b 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -79,25 +79,18 @@ public: /*@{*/ void clearWaypointList(); ///< Sends the waypoint clear all message to the MAV void readWaypoints(); ///< Requests the MAV's current waypoint list - void writeWaypoints(); ///< Sends the local waypoint list to the MAV + void writeWaypoints(); ///< Sends the waypoint list to the MAV int setCurrentWaypoint(quint16 seq); ///< Changes the current waypoint and sends the sequence number of the waypoint that should get the new target waypoint to the UAS /*@}*/ - /** @name Local waypoint list operations */ + /** @name Waypoint list operations */ /*@{*/ - const QVector &getWaypointList(void) { return waypoints; } ///< Returns a const reference to the local waypoint list. - void localAddWaypoint(Waypoint *wp); ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly - int localRemoveWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage - void localMoveWaypoint(quint16 cur_seq, quint16 new_seq); ///< locally move a waypoint from its current position cur_seq to a new position new_seq - void localSaveWaypoints(const QString &saveFile); ///< saves the local waypoint list to saveFile - void localLoadWaypoints(const QString &loadFile); ///< loads a waypoint list from loadFile - /*@}*/ - - /** @name Global waypoint list operations */ - /*@{*/ - const QVector &getGlobalWaypointList(void) { return waypoints; } ///< Returns a const reference to the global waypoint list. - void globalAddWaypoint(Waypoint *wp); ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly - int globalRemoveWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage + const QVector &getWaypointList(void) { return waypoints; } ///< Returns a const reference to the waypoint list. + void addWaypoint(Waypoint *wp); ///< adds a new waypoint to the end of the list and changes its sequence number accordingly + int removeWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage + void moveWaypoint(quint16 cur_seq, quint16 new_seq); ///< locally move a waypoint from its current position cur_seq to a new position new_seq + void saveWaypoints(const QString &saveFile); ///< saves the local waypoint list to saveFile + void loadWaypoints(const QString &loadFile); ///< loads a waypoint list from loadFile /*@}*/ private: diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index bf01124b9984179fccf0052448b26017430e09b0..b04969fa262bd484f1f888ccb022e450be783730 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -206,13 +206,13 @@ void MainWindow::connectWidgets() // 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, QPointF)), waypointsDockWidget->widget(), SLOT(setIsWPGlobal(bool, QPointF))); - connect(mapWidget, SIGNAL(sendGeometryEndDrag(QPointF,int)), waypointsDockWidget->widget(), SLOT(waypointGlobalChanged(QPointF,int)) ); + //connect(mapWidget, SIGNAL(createGlobalWP(bool, QPointF)), waypointsDockWidget->widget(), SLOT(setIsWPGlobal(bool, QPointF))); + //connect(mapWidget, SIGNAL(sendGeometryEndDrag(QPointF,int)), waypointsDockWidget->widget(), SLOT(waypointGlobalChanged(QPointF,int)) ); // it notifies that a waypoint global goes to do create and a map graphic too connect(waypointsDockWidget->widget(), SIGNAL(createWaypointAtMap(QPointF)), mapWidget, SLOT(createWaypointGraphAtMap(QPointF))); // it notifies that a waypoint global change it´s position by spinBox on Widget WaypointView - connect(waypointsDockWidget->widget(), SIGNAL(changePositionWPGlobalBySpinBox(int,float,float)), mapWidget, SLOT(changeGlobalWaypointPositionBySpinBox(int,float,float))); + //connect(waypointsDockWidget->widget(), SIGNAL(changePositionWPGlobalBySpinBox(int,float,float)), mapWidget, SLOT(changeGlobalWaypointPositionBySpinBox(int,float,float))); } } diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 1f7f1332338fc7a834f93607dc78a43eebf55d4a..8a9e921a173b8184193a20343c8286885dbfc1fb 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -37,7 +37,6 @@ This file is part of the PIXHAWK project #include #include #include -#include "WaypointGlobalView.h" #include #include @@ -89,8 +88,6 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : updateStatusLabel(""); this->setVisible(false); - isGlobalWP = false; - isLocalWP = false; centerMapCoordinate.setX(0.0); centerMapCoordinate.setY(0.0); @@ -101,7 +98,7 @@ WaypointList::~WaypointList() delete m_ui; } -void WaypointList::updateLocalPosition(UASInterface* uas, double x, double y, double z, quint64 usec) +void WaypointList::updatePosition(UASInterface* uas, double x, double y, double z, quint64 usec) { Q_UNUSED(uas); Q_UNUSED(usec); @@ -139,7 +136,7 @@ void WaypointList::saveWaypoints() if (uas) { QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)")); - uas->getWaypointManager().localSaveWaypoints(fileName); + uas->getWaypointManager().saveWaypoints(fileName); } } @@ -148,7 +145,7 @@ void WaypointList::loadWaypoints() if (uas) { QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)")); - uas->getWaypointManager().localLoadWaypoints(fileName); + uas->getWaypointManager().loadWaypoints(fileName); } } @@ -172,40 +169,41 @@ void WaypointList::add() { if (uas) { - if(isGlobalWP) - { - const QVector &waypoints = uas->getWaypointManager().getWaypointList(); - if (waypoints.size() > 0) - { - Waypoint *last = waypoints.at(waypoints.size()-1); - Waypoint *wp = new Waypoint(0, centerMapCoordinate.x(), centerMapCoordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()); - uas->getWaypointManager().localAddWaypoint(wp); - } - else - { - Waypoint *wp = new Waypoint(0, centerMapCoordinate.x(), centerMapCoordinate.y(), -0.8, 0.0, true, true, 0.15, 2000); - uas->getWaypointManager().localAddWaypoint(wp); - } - - emit createWaypointAtMap(centerMapCoordinate); - } - else +// if(isGlobalWP) +// { +// const QVector &waypoints = uas->getWaypointManager().getWaypointList(); +// if (waypoints.size() > 0) +// { +// Waypoint *last = waypoints.at(waypoints.size()-1); +// Waypoint *wp = new Waypoint(0, centerMapCoordinate.x(), centerMapCoordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()); +// uas->getWaypointManager().addWaypoint(wp); +// } +// else +// { +// Waypoint *wp = new Waypoint(0, centerMapCoordinate.x(), centerMapCoordinate.y(), -0.8, 0.0, true, true, 0.15, 2000); +// uas->getWaypointManager().addWaypoint(wp); +// } +// +// emit createWaypointAtMap(centerMapCoordinate); +// } +// else { 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); + Waypoint *wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), + last->getHoldTime(), last->getFrame(), last->getAction()); + uas->getWaypointManager().addWaypoint(wp); } else { - isLocalWP = true; + //isLocalWP = true; Waypoint *wp = new Waypoint(0, 1.1, 1.1, -0.8, 0.0, true, true, 0.15, 2000); - uas->getWaypointManager().localAddWaypoint(wp); + uas->getWaypointManager().addWaypoint(wp); } @@ -215,31 +213,33 @@ void WaypointList::add() void WaypointList::addCurrentPositonWaypoint() { + /* TODO: implement with new waypoint structure if (uas) { // For Global Waypoints - if(isGlobalWP) - { - isLocalWP = false; - } - else + //if(isGlobalWP) + //{ + //isLocalWP = false; + //} + //else { 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); + uas->getWaypointManager().addWaypoint(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); + uas->getWaypointManager().addWaypoint(wp); } - isLocalWP = true; + //isLocalWP = true; } } + */ } void WaypointList::updateStatusLabel(const QString &string) @@ -287,59 +287,59 @@ void WaypointList::waypointListChanged() const QVector &waypoints = uas->getWaypointManager().getWaypointList(); // For Global Waypoints - if(isGlobalWP) - { - isLocalWP = false; - // first remove all views of non existing waypoints - if (!wpGlobalViews.empty()) - { - QMapIterator viewIt(wpGlobalViews); - 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()) - { - WaypointGlobalView* widget = wpGlobalViews.find(cur).value(); - widget->hide(); - listLayout->removeWidget(widget); - wpGlobalViews.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 (!wpGlobalViews.contains(wp)) - { - WaypointGlobalView* wpview = new WaypointGlobalView(wp, this); - wpGlobalViews.insert(wp, wpview); - connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); - connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(changeWPPositionBySpinBox(Waypoint*))); -// connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); -// connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); - // connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(waypointGlobalPositionChanged(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); - } - - } - else + //if(isGlobalWP) + //{ + //isLocalWP = false; + //// first remove all views of non existing waypoints + //if (!wpGlobalViews.empty()) + //{ + //QMapIterator viewIt(wpGlobalViews); + //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()) + //{ + //WaypointGlobalView* widget = wpGlobalViews.find(cur).value(); + //widget->hide(); + //listLayout->removeWidget(widget); + //wpGlobalViews.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 (!wpGlobalViews.contains(wp)) + //{ + //WaypointGlobalView* wpview = new WaypointGlobalView(wp, this); + //wpGlobalViews.insert(wp, wpview); + //connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); + //connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(changeWPPositionBySpinBox(Waypoint*))); +//// connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); +//// connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); + //// connect(wpview, SIGNAL(changePositionWP(Waypoint*)), this, SLOT(waypointGlobalPositionChanged(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); + //} + + //} + //else { // for local Waypoints // first remove all views of non existing waypoints @@ -414,7 +414,7 @@ void WaypointList::moveUp(Waypoint* wp) // if wp was found and its not the first entry, move it if (i < waypoints.size() && i > 0) { - uas->getWaypointManager().localMoveWaypoint(i, i-1); + uas->getWaypointManager().moveWaypoint(i, i-1); } } } @@ -436,7 +436,7 @@ void WaypointList::moveDown(Waypoint* wp) // if wp was found and its not the last entry, move it if (i < waypoints.size()-1) { - uas->getWaypointManager().localMoveWaypoint(i, i+1); + uas->getWaypointManager().moveWaypoint(i, i+1); } } } @@ -445,7 +445,7 @@ void WaypointList::removeWaypoint(Waypoint* wp) { if (uas) { - uas->getWaypointManager().localRemoveWaypoint(wp->getId()); + uas->getWaypointManager().removeWaypoint(wp->getId()); } } @@ -467,43 +467,13 @@ 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 + emit clearPathclicked(); + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); + while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++) { - - - 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(); - - - } - + WaypointView* widget = wpViews.find(waypoints[0]).value(); + widget->remove(); } - } } @@ -517,52 +487,16 @@ void WaypointList::addWaypointMouse(QPointF coordinate) { 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); + uas->getWaypointManager().addWaypoint(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 Notifies the user that a global waypoint will be created */ -void WaypointList::setIsWPGlobal(bool value, QPointF centerCoordinate) -{ - centerMapCoordinate = centerCoordinate; - - - if(isLocalWP) - { - if(wpViews.size()!= 0) - { - - int ret = QMessageBox::warning(this, tr("QGroundControl"), - tr("There are Local Waypoints created.\n" - "Do you want to clear them?"), - QMessageBox::Ok | QMessageBox::Cancel); - - if(ret) - { - clearLocalWPWidget(); - - - } - + uas->getWaypointManager().addWaypoint(wp); } - isLocalWP = !value; - isGlobalWP = value; } - else - { - isGlobalWP = value; - } } @@ -580,8 +514,8 @@ void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP) temp->setX(coordinate.x()); temp->setY(coordinate.y()); - WaypointGlobalView* widget = wpGlobalViews.find(waypoints[indexWP]).value(); - widget->updateValues(); + //WaypointGlobalView* widget = wpGlobalViews.find(waypoints[indexWP]).value(); + //widget->updateValues(); } } @@ -601,24 +535,16 @@ void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP) //} -void WaypointList::clearLocalWPWidget() +void WaypointList::clearWPWidget() { - 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(); - - } - } -} - -void WaypointList::changeWPPositionBySpinBox(Waypoint *wp) -{ - - emit changePositionWPGlobalBySpinBox(wp->getId(), wp->getY(), wp->getX()); + 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(); + } + } + //emit changePositionWPBySpinBox(wp->getId(), wp->getY(), wp->getX()); } diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index 24325046f241622707310c749abdc34d2f58153c..ab5d760484d416cdb21b025a1ec00e3a616ed4b4 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -56,7 +56,7 @@ class WaypointList : public QWidget { virtual ~WaypointList(); public slots: - void updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec); + void updatePosition(UASInterface*, double x, double y, double z, quint64 usec); void updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64 usec); void setUAS(UASInterface* uas); @@ -77,7 +77,7 @@ public slots: /** @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, QPointF centerCoordinate); + //void setIsWPGlobal(bool value, QPointF centerCoordinate); //Update events @@ -93,9 +93,9 @@ public slots: /** @brief The MapWidget informs that a waypoint global was changed on the map */ void waypointGlobalChanged(const QPointF coordinate, const int indexWP); - void clearLocalWPWidget(); + void clearWPWidget(); - void changeWPPositionBySpinBox(Waypoint* wp); + //void changeWPPositionBySpinBox(Waypoint* wp); // Waypoint operations void moveUp(Waypoint* wp); @@ -109,7 +109,7 @@ signals: void clearPathclicked(); void createWaypointAtMap(const QPointF coordinate); // void ChangeWaypointGlobalPosition(int index, QPointF coord); - void changePositionWPGlobalBySpinBox(int indexWP, float lat, float lon); + void changePositionWPBySpinBox(int indexWP, float lat, float lon); @@ -118,15 +118,13 @@ protected: protected: QMap wpViews; - QMap wpGlobalViews; + //QMap wpGlobalViews; QVBoxLayout* listLayout; UASInterface* uas; double mavX; double mavY; double mavZ; double mavYaw; - bool isGlobalWP; - bool isLocalWP; QPointF centerMapCoordinate; private: diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index 4e9ee95db7d2ff5454cbee3172f6c98bd31285af..7e5244a8b2f08c016da2f74069b586aff24f58b1 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointView.cc @@ -46,16 +46,32 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : m_ui->setupUi(this); this->wp = wp; - wp->setType(Waypoint::LOCAL); - m_ui->orbitSpinBox->setEnabled(false); - m_ui->orbitSpinBox->hide(); + wp->setFrame(MAV_FRAME_LOCAL); + + // add actions + m_ui->comboBox_action->addItem("Navigate",MAV_ACTION_NAVIGATE); + m_ui->comboBox_action->addItem("TakeOff",MAV_ACTION_TAKEOFF); + m_ui->comboBox_action->addItem("Land",MAV_ACTION_LAND); + m_ui->comboBox_action->addItem("Loiter",MAV_ACTION_LOITER); + + // add frames + m_ui->comboBox_frame->addItem("Global",MAV_FRAME_GLOBAL); + m_ui->comboBox_frame->addItem("Local",MAV_FRAME_LOCAL); + + // defaults + changedAction(0); + changedFrame(0); // 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))); + connect(m_ui->posNSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double))); + connect(m_ui->posESpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double))); + connect(m_ui->posDSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setZ(double))); + + connect(m_ui->lonSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double))); + connect(m_ui->latSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double))); + connect(m_ui->altSpinBox, 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))); @@ -67,9 +83,8 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : 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->orbitCheckBox, SIGNAL(stateChanged(int)), this, SLOT(changeOrbitalState(int))); - - + connect(m_ui->comboBox_action, SIGNAL(activated(int)), this, SLOT(changedAction(int))); + connect(m_ui->comboBox_frame, SIGNAL(activated(int)), this, SLOT(changedFrame(int))); connect(m_ui->orbitSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setOrbit(double))); connect(m_ui->holdTimeSpinBox, SIGNAL(valueChanged(int)), wp, SLOT(setHoldTime(int))); @@ -104,6 +119,72 @@ void WaypointView::changedAutoContinue(int state) wp->setAutocontinue(true); } +void WaypointView::changedAction(int index) +{ + // set action for waypoint + + + // hide everything to start + m_ui->orbitSpinBox->hide(); + m_ui->takeOffAngleSpinBox->hide(); + m_ui->autoContinue->hide(); + m_ui->holdTimeSpinBox->hide(); + + // set waypoint action + MAV_ACTION action = (MAV_ACTION)m_ui->comboBox_action->itemData(index).toUInt(); + wp->setAction(action); + + // expose ui based on action + switch(action) + { + case MAV_ACTION_TAKEOFF: + m_ui->takeOffAngleSpinBox->show(); + break; + case MAV_ACTION_LAND: + break; + case MAV_ACTION_NAVIGATE: + m_ui->autoContinue->show(); + break; + case MAV_ACTION_LOITER: + m_ui->orbitSpinBox->show(); + m_ui->holdTimeSpinBox->show(); + break; + default: + std::cerr << "unknown action" << std::endl; + } +} + +void WaypointView::changedFrame(int index) +{ + // hide everything to start + m_ui->lonSpinBox->hide(); + m_ui->latSpinBox->hide(); + m_ui->altSpinBox->hide(); + m_ui->posNSpinBox->hide(); + m_ui->posESpinBox->hide(); + m_ui->posDSpinBox->hide(); + + // set waypoint action + MAV_FRAME frame = (MAV_FRAME)m_ui->comboBox_frame->itemData(index).toUInt(); + wp->setFrame(frame); + + switch(frame) + { + case MAV_FRAME_GLOBAL: + m_ui->lonSpinBox->show(); + m_ui->latSpinBox->show(); + m_ui->altSpinBox->show(); + break; + case MAV_FRAME_LOCAL: + m_ui->posNSpinBox->show(); + m_ui->posESpinBox->show(); + m_ui->posDSpinBox->show(); + break; + default: + std::cerr << "unknown frame" << std::endl; + } +} + void WaypointView::changedCurrent(int state) { if (state == 0) @@ -121,9 +202,41 @@ void WaypointView::changedCurrent(int state) void WaypointView::updateValues() { - m_ui->xSpinBox->setValue(wp->getX()); - m_ui->ySpinBox->setValue(wp->getY()); - m_ui->zSpinBox->setValue(wp->getZ()); + // update frame + MAV_FRAME frame = wp->getFrame(); + changedFrame(m_ui->comboBox_frame->findData(frame)); + switch(frame) + { + case(MAV_FRAME_LOCAL): + m_ui->posNSpinBox->setValue(wp->getX()); + m_ui->posESpinBox->setValue(wp->getY()); + m_ui->posDSpinBox->setValue(wp->getZ()); + break; + case(MAV_FRAME_GLOBAL): + m_ui->lonSpinBox->setValue(wp->getX()); + m_ui->latSpinBox->setValue(wp->getY()); + m_ui->altSpinBox->setValue(wp->getZ()); + break; + } + + // update action + MAV_ACTION action = wp->getAction(); + changedFrame(m_ui->comboBox_frame->findData(frame)); + changedAction(m_ui->comboBox_action->findData(action)); + switch(action) + { + case MAV_ACTION_TAKEOFF: + break; + case MAV_ACTION_LAND: + break; + case MAV_ACTION_NAVIGATE: + break; + case MAV_ACTION_LOITER: + break; + default: + std::cerr << "unknown action" << std::endl; + } + m_ui->yawSpinBox->setValue(wp->getYaw()/M_PI*180.); m_ui->selectedBox->setChecked(wp->getCurrent()); m_ui->autoContinue->setChecked(wp->getAutoContinue()); @@ -159,21 +272,3 @@ void WaypointView::changeEvent(QEvent *e) break; } } - -void WaypointView::changeOrbitalState(int state) -{ - Q_UNUSED(state); - - if(m_ui->orbitCheckBox->isChecked()) - { - m_ui->orbitSpinBox->setEnabled(true); - m_ui->orbitSpinBox->show(); - wp->setType(Waypoint::LOCAL_ORBIT); - } - else - { - m_ui->orbitSpinBox->setEnabled(false); - m_ui->orbitSpinBox->hide(); - wp->setType(Waypoint::LOCAL); - } -} diff --git a/src/ui/WaypointView.h b/src/ui/WaypointView.h index 36710213596d2c0e26f914e63a512e7ae5269088..8a1058bde33105b5ba2d31c027bd3a3a849386cc 100644 --- a/src/ui/WaypointView.h +++ b/src/ui/WaypointView.h @@ -36,6 +36,7 @@ This file is part of the QGROUNDCONTROL project #include #include "Waypoint.h" +#include namespace Ui { class WaypointView; @@ -56,7 +57,8 @@ public slots: void moveDown(); void remove(); void changedAutoContinue(int); - void changeOrbitalState(int state); + void changedFrame(int state); + void changedAction(int state); void changedCurrent(int); void updateValues(void); diff --git a/src/ui/WaypointView.ui b/src/ui/WaypointView.ui index 4396337c09a5b3ca12524279cda980269b12aef5..6918d631120a982e2c4dca01ec4b8f369ab2ba59 100644 --- a/src/ui/WaypointView.ui +++ b/src/ui/WaypointView.ui @@ -6,12 +6,12 @@ 0 0 - 853 - 30 + 1389 + 33 - + 0 0 @@ -152,10 +152,16 @@ QProgressBar::chunk#thrustBar { + + + 0 + 0 + + - + 2 @@ -198,31 +204,47 @@ QProgressBar::chunk#thrustBar { - - - Qt::Horizontal + + + + 0 + 0 + - - - 25 - 0 - + + + + + + + 0 + 0 + - + - + + + + 0 + 0 + + Position X coordinate + + N + m - -1000.000000000000000 + -10000.000000000000000 - 1000.000000000000000 + 10000.000000000000000 0.050000000000000 @@ -230,18 +252,27 @@ QProgressBar::chunk#thrustBar { - + + + + 0 + 0 + + Position Y coordinate + + E + m - -1000.000000000000000 + -10000.000000000000000 - 1000.000000000000000 + 10000.000000000000000 0.050000000000000 @@ -252,26 +283,116 @@ QProgressBar::chunk#thrustBar { - + + + + 0 + 0 + + Position Z coordinate (negative) + + D + m - -1000.000000000000000 + -10000.000000000000000 - 0.000000000000000 + 10000.000000000000000 0.050000000000000 + + + + + 0 + 0 + + + + lat + + + ° + + + 7 + + + -90.000000000000000 + + + 90.000000000000000 + + + + + + + + 0 + 0 + + + + lon + + + ° + + + 7 + + + -180.000000000000000 + + + 180.000000000000000 + + + + + + + + 0 + 0 + + + + alt + + + m + + + 7 + + + -100000.000000000000000 + + + 100000.000000000000000 + + + + + + 0 + 0 + + Yaw angle @@ -294,6 +415,12 @@ QProgressBar::chunk#thrustBar { + + + 0 + 0 + + Orbit radius @@ -304,15 +431,24 @@ QProgressBar::chunk#thrustBar { 0.050000000000000 - 1.000000000000000 + 100.000000000000000 0.050000000000000 + + 20.000000000000000 + + + + 0 + 0 + + Time in milliseconds that the MAV has to stay inside the orbit before advancing @@ -334,19 +470,28 @@ QProgressBar::chunk#thrustBar { - + + + + 0 + 0 + + - Automatically continue after this waypoint + Take off angle - + + + ° + - + - enable orbital mode (loiter at waypoint) + Automatically continue after this waypoint