diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index dd695372675cef01317a10f7ceaa8b90cbb75c95..9e1ddc6d1c09dc10a56c3ab56280042d3dc69027 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -172,8 +172,7 @@ FORMS += src/ui/MainWindow.ui \ src/ui/Linechart.ui \ src/ui/UASView.ui \ src/ui/ParameterInterface.ui \ - src/ui/WaypointList.ui \ - src/ui/WaypointView.ui \ + src/ui/WaypointList.ui \ src/ui/ObjectDetectionView.ui \ src/ui/JoystickWidget.ui \ src/ui/DebugConsole.ui \ @@ -209,7 +208,9 @@ FORMS += src/ui/MainWindow.ui \ src/ui/mission/QGCMissionConditionWidget.ui \ src/ui/map/QGCMapTool.ui \ src/ui/map/QGCMapToolBar.ui \ - src/ui/QGCMAVLinkInspector.ui + src/ui/QGCMAVLinkInspector.ui \ + src/ui/WaypointViewOnlyView.ui \ + src/ui/WaypointEditableView.ui INCLUDEPATH += src \ src/ui \ src/ui/linechart \ @@ -255,8 +256,7 @@ HEADERS += src/MG.h \ src/comm/UDPLink.h \ src/ui/ParameterInterface.h \ src/ui/WaypointList.h \ - src/Waypoint.h \ - src/ui/WaypointView.h \ + src/Waypoint.h \ src/ui/ObjectDetectionView.h \ src/input/JoystickInput.h \ src/ui/JoystickWidget.h \ @@ -322,7 +322,10 @@ HEADERS += src/MG.h \ src/QGCGeo.h \ src/ui/QGCToolBar.h \ src/ui/QGCMAVLinkInspector.h \ - src/ui/MAVLinkDecoder.h + src/ui/MAVLinkDecoder.h \ + src/ui/WaypointViewOnlyView.h \ + src/ui/WaypointViewOnlyView.h \ + src/ui/WaypointEditableView.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h @@ -384,7 +387,6 @@ SOURCES += src/main.cc \ src/ui/ParameterInterface.cc \ src/ui/WaypointList.cc \ src/Waypoint.cc \ - src/ui/WaypointView.cc \ src/ui/ObjectDetectionView.cc \ src/input/JoystickInput.cc \ src/ui/JoystickWidget.cc \ @@ -447,7 +449,9 @@ SOURCES += src/main.cc \ src/ui/map/QGCMapToolBar.cc \ src/ui/QGCToolBar.cc \ src/ui/QGCMAVLinkInspector.cc \ - src/ui/MAVLinkDecoder.cc + src/ui/MAVLinkDecoder.cc \ + src/ui/WaypointViewOnlyView.cc \ + src/ui/WaypointEditableView.cc # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler macx|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc diff --git a/src/Waypoint.cc b/src/Waypoint.cc index 93cd17a6d9204caa16e375ed0d284709379bd29d..1f42cd364cd1d477f2e234b2c9201feeaaaf816c 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -206,7 +206,7 @@ void Waypoint::setFrame(MAV_FRAME frame) void Waypoint::setAutocontinue(bool autoContinue) { - if (this->autocontinue != autocontinue) { + if (this->autocontinue != autoContinue) { this->autocontinue = autoContinue; emit changed(this); } diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 1508793cf485c5a9188276a189852c6aa723cec0..e246821d2c7447138b071fa937f78f1cb7136048 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -46,7 +46,7 @@ UASWaypointManager::UASWaypointManager(UAS &_uas) current_partner_systemid(0), current_partner_compid(0), protocol_timer(this), - currentWaypoint(NULL) + currentWaypointEditable(NULL) { connect(&protocol_timer, SIGNAL(timeout()), this, SLOT(timeout())); connect(&uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(handleLocalPositionChanged(UASInterface*,double,double,double,quint64))); @@ -92,11 +92,11 @@ void UASWaypointManager::handleLocalPositionChanged(UASInterface* mav, double x, { Q_UNUSED(mav); Q_UNUSED(time); - if (waypoints.count() > 0 && currentWaypoint && (currentWaypoint->getFrame() == MAV_FRAME_LOCAL_NED || currentWaypoint->getFrame() == MAV_FRAME_LOCAL_ENU)) + if (waypointsEditable.count() > 0 && currentWaypointEditable && (currentWaypointEditable->getFrame() == MAV_FRAME_LOCAL_NED || currentWaypointEditable->getFrame() == MAV_FRAME_LOCAL_ENU)) { - double xdiff = x-currentWaypoint->getX(); - double ydiff = y-currentWaypoint->getY(); - double zdiff = z-currentWaypoint->getZ(); + double xdiff = x-currentWaypointEditable->getX(); + double ydiff = y-currentWaypointEditable->getY(); + double zdiff = z-currentWaypointEditable->getZ(); double dist = sqrt(xdiff*xdiff + ydiff*ydiff + zdiff*zdiff); emit waypointDistanceChanged(dist); } @@ -144,9 +144,17 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ if(wp->seq == current_wp_id) { //// // qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->param4 << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << "Frame:"<< (MAV_FRAME) wp->frame << "Command:" << (MAV_CMD) wp->command; - Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, (MAV_FRAME) wp->frame, (MAV_CMD) wp->command); - addWaypoint(lwp, false); - if (wp->current == 1) currentWaypoint = lwp; + + Waypoint *lwp_vo = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, (MAV_FRAME) wp->frame, (MAV_CMD) wp->command); + addWaypointViewOnly(lwp_vo); + + + if (read_to_edit == true) { + Waypoint *lwp_ed = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->param1, wp->param2, wp->param3, wp->param4, wp->autocontinue, wp->current, (MAV_FRAME) wp->frame, (MAV_CMD) wp->command); + addWaypointEditable(lwp_ed, false); + if (wp->current == 1) currentWaypointEditable = lwp_ed; + } + //get next waypoint current_wp_id++; @@ -165,7 +173,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ protocol_timer.stop(); emit readGlobalWPFromUAS(false); - if (currentWaypoint) emit currentWaypointChanged(currentWaypoint->getId()); + if (currentWaypointEditable) emit currentWaypointChanged(currentWaypointEditable->getId()); emit updateStatusString("done."); // // qDebug() << "got all waypoints from ID " << systemId; @@ -230,14 +238,13 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m current_state = WP_IDLE; // update the local main storage - if (wpc->seq < waypoints.size()) { - for(int i = 0; i < waypoints.size(); i++) { - if (waypoints[i]->getId() == wpc->seq) { - waypoints[i]->setCurrent(true); - currentWaypoint = waypoints[i]; - + if (wpc->seq < waypointsViewOnly.size()) { + for(int i = 0; i < waypointsViewOnly.size(); i++) { + if (waypointsViewOnly[i]->getId() == wpc->seq) { + waypointsViewOnly[i]->setCurrent(true); + //currentWaypointEditable = waypoints[i]; } else { - waypoints[i]->setCurrent(false); + waypointsViewOnly[i]->setCurrent(false); } } } @@ -248,33 +255,46 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m } } -void UASWaypointManager::notifyOfChange(Waypoint* wp) +void UASWaypointManager::notifyOfChangeEditable(Waypoint* wp) { // // qDebug() << "WAYPOINT CHANGED: ID:" << wp->getId(); // If only one waypoint was changed, emit only WP signal if (wp != NULL) { - emit waypointChanged(uas.getUASID(), wp); + emit waypointEditableChanged(uas.getUASID(), wp); + } else { + emit waypointEditableListChanged(); + emit waypointEditableListChanged(uas.getUASID()); + } +} + +void UASWaypointManager::notifyOfChangeViewOnly(Waypoint* wp) +{ + if (wp != NULL) { + emit waypointViewOnlyChanged(uas.getUASID(), wp); } else { - emit waypointListChanged(); - emit waypointListChanged(uas.getUASID()); + emit waypointViewOnlyListChanged(); + emit waypointViewOnlyListChanged(uas.getUASID()); } } + int UASWaypointManager::setCurrentWaypoint(quint16 seq) { - if (seq < waypoints.size()) { + if (seq < waypointsViewOnly.size()) { if(current_state == WP_IDLE) { + + /* //update local main storage - for(int i = 0; i < waypoints.size(); i++) { - if (waypoints[i]->getId() == seq) { - waypoints[i]->setCurrent(true); - currentWaypoint = waypoints[i]; + for(int i = 0; i < waypointsViewOnly.size(); i++) { + if (waypointsViewOnly[i]->getId() == seq) { + waypointsViewOnly[i]->setCurrent(true); + //currentWaypointEditable = waypoints[i]; } else { - waypoints[i]->setCurrent(false); + waypointsViewOnly[i]->setCurrent(false); } } + */ - //TODO: signal changed waypoint list //send change to UAS - important to note: if the transmission fails, we have inconsistencies protocol_timer.start(PROTOCOL_TIMEOUT_MS); @@ -295,26 +315,59 @@ int UASWaypointManager::setCurrentWaypoint(quint16 seq) return -1; } +int UASWaypointManager::setCurrentEditable(quint16 seq) +{ + if (seq < waypointsEditable.size()) { + if(current_state == WP_IDLE) { + //update local main storage + for(int i = 0; i < waypointsEditable.size(); i++) { + if (waypointsEditable[i]->getId() == seq) { + waypointsEditable[i]->setCurrent(true); + //currentWaypointEditable = waypoints[i]; + } else { + waypointsEditable[i]->setCurrent(false); + } + } + + return 0; + } + } + return -1; +} + +void UASWaypointManager::addWaypointViewOnly(Waypoint *wp) +{ + if (wp) + { + waypointsViewOnly.insert(waypointsViewOnly.size(), wp); + connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeViewOnly(Waypoint*))); + + emit waypointViewOnlyListChanged(); + emit waypointViewOnlyListChanged(uas.getUASID()); + } +} + + /** * @warning Make sure the waypoint stays valid for the whole application lifecycle! * @param enforceFirstActive Enforces that the first waypoint is set as active * @see createWaypoint() is more suitable for most use cases */ -void UASWaypointManager::addWaypoint(Waypoint *wp, bool enforceFirstActive) +void UASWaypointManager::addWaypointEditable(Waypoint *wp, bool enforceFirstActive) { if (wp) { - wp->setId(waypoints.size()); - if (enforceFirstActive && waypoints.size() == 0) + wp->setId(waypointsEditable.size()); + if (enforceFirstActive && waypointsEditable.size() == 0) { wp->setCurrent(true); - currentWaypoint = wp; + currentWaypointEditable = wp; } - waypoints.insert(waypoints.size(), wp); - connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChange(Waypoint*))); + waypointsEditable.insert(waypointsEditable.size(), wp); + connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*))); - emit waypointListChanged(); - emit waypointListChanged(uas.getUASID()); + emit waypointEditableListChanged(); + emit waypointEditableListChanged(uas.getUASID()); } } @@ -324,36 +377,36 @@ void UASWaypointManager::addWaypoint(Waypoint *wp, bool enforceFirstActive) Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive) { Waypoint* wp = new Waypoint(); - wp->setId(waypoints.size()); - if (enforceFirstActive && waypoints.size() == 0) + wp->setId(waypointsEditable.size()); + if (enforceFirstActive && waypointsEditable.size() == 0) { wp->setCurrent(true); - currentWaypoint = wp; + currentWaypointEditable = wp; } - waypoints.insert(waypoints.size(), wp); - connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChange(Waypoint*))); + waypointsEditable.insert(waypointsEditable.size(), wp); + connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*))); - emit waypointListChanged(); - emit waypointListChanged(uas.getUASID()); + emit waypointEditableListChanged(); + emit waypointEditableListChanged(uas.getUASID()); return wp; } int UASWaypointManager::removeWaypoint(quint16 seq) { - if (seq < waypoints.size()) + if (seq < waypointsEditable.size()) { - Waypoint *t = waypoints[seq]; - waypoints.remove(seq); + Waypoint *t = waypointsEditable[seq]; + waypointsEditable.remove(seq); delete t; t = NULL; - for(int i = seq; i < waypoints.size(); i++) + for(int i = seq; i < waypointsEditable.size(); i++) { - waypoints[i]->setId(i); + waypointsEditable[i]->setId(i); } - emit waypointListChanged(); - emit waypointListChanged(uas.getUASID()); + emit waypointEditableListChanged(); + emit waypointEditableListChanged(uas.getUASID()); return 0; } return -1; @@ -361,26 +414,26 @@ int UASWaypointManager::removeWaypoint(quint16 seq) void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq) { - if (cur_seq != new_seq && cur_seq < waypoints.size() && new_seq < waypoints.size()) + if (cur_seq != new_seq && cur_seq < waypointsEditable.size() && new_seq < waypointsEditable.size()) { - Waypoint *t = waypoints[cur_seq]; + Waypoint *t = waypointsEditable[cur_seq]; if (cur_seq < new_seq) { for (int i = cur_seq; i < new_seq; i++) { - waypoints[i] = waypoints[i+1]; + waypointsEditable[i] = waypointsEditable[i+1]; } } else { for (int i = cur_seq; i > new_seq; i--) { - waypoints[i] = waypoints[i-1]; + waypointsEditable[i] = waypointsEditable[i-1]; } } - waypoints[new_seq] = t; + waypointsEditable[new_seq] = t; - emit waypointListChanged(); - emit waypointListChanged(uas.getUASID()); + emit waypointEditableListChanged(); + emit waypointEditableListChanged(uas.getUASID()); } } @@ -395,10 +448,10 @@ void UASWaypointManager::saveWaypoints(const QString &saveFile) //write the waypoint list version to the first line for compatibility check out << "QGC WPL 110\r\n"; - for (int i = 0; i < waypoints.size(); i++) + for (int i = 0; i < waypointsEditable.size(); i++) { - waypoints[i]->setId(i); - waypoints[i]->save(out); + waypointsEditable[i]->setId(i); + waypointsEditable[i]->save(out); } file.close(); } @@ -409,9 +462,9 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile) if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) return; - while(waypoints.size()>0) { - Waypoint *t = waypoints[0]; - waypoints.remove(0); + while(waypointsEditable.size()>0) { + Waypoint *t = waypointsEditable[0]; + waypointsEditable.remove(0); delete t; } @@ -430,8 +483,8 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile) Waypoint *t = new Waypoint(); if(t->load(in)) { - t->setId(waypoints.size()); - waypoints.insert(waypoints.size(), t); + t->setId(waypointsEditable.size()); + waypointsEditable.insert(waypointsEditable.size(), t); } else { @@ -445,8 +498,8 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile) file.close(); emit loadWPFile(); - emit waypointListChanged(); - emit waypointListChanged(uas.getUASID()); + emit waypointEditableListChanged(); + emit waypointEditableListChanged(uas.getUASID()); } void UASWaypointManager::clearWaypointList() @@ -471,7 +524,7 @@ const QVector UASWaypointManager::getGlobalFrameWaypointList() // with complete waypoint list // instead of filtering on each request QVector wps; - foreach (Waypoint* wp, waypoints) + foreach (Waypoint* wp, waypointsEditable) { if (wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { @@ -487,7 +540,7 @@ const QVector UASWaypointManager::getGlobalFrameAndNavTypeWaypointLi // with complete waypoint list // instead of filtering on each request QVector wps; - foreach (Waypoint* wp, waypoints) + foreach (Waypoint* wp, waypointsEditable) { if ((wp->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && wp->isNavigationType()) { @@ -503,7 +556,7 @@ const QVector UASWaypointManager::getNavTypeWaypointList() // with complete waypoint list // instead of filtering on each request QVector wps; - foreach (Waypoint* wp, waypoints) + foreach (Waypoint* wp, waypointsEditable) { if (wp->isNavigationType()) { @@ -515,15 +568,15 @@ const QVector UASWaypointManager::getNavTypeWaypointList() int UASWaypointManager::getIndexOf(Waypoint* wp) { - return waypoints.indexOf(wp); + return waypointsEditable.indexOf(wp); } int UASWaypointManager::getGlobalFrameIndexOf(Waypoint* wp) { - // Search through all waypoints, + // Search through all waypointsEditable, // counting only those in global frame int i = 0; - foreach (Waypoint* p, waypoints) { + foreach (Waypoint* p, waypointsEditable) { if (p->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { if (p == wp) @@ -539,10 +592,10 @@ int UASWaypointManager::getGlobalFrameIndexOf(Waypoint* wp) int UASWaypointManager::getGlobalFrameAndNavTypeIndexOf(Waypoint* wp) { - // Search through all waypoints, + // Search through all waypointsEditable, // counting only those in global frame int i = 0; - foreach (Waypoint* p, waypoints) { + foreach (Waypoint* p, waypointsEditable) { if ((p->getFrame() == MAV_FRAME_GLOBAL || wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && p->isNavigationType()) { if (p == wp) @@ -558,10 +611,10 @@ int UASWaypointManager::getGlobalFrameAndNavTypeIndexOf(Waypoint* wp) int UASWaypointManager::getNavTypeIndexOf(Waypoint* wp) { - // Search through all waypoints, + // Search through all waypointsEditable, // counting only those in global frame int i = 0; - foreach (Waypoint* p, waypoints) + foreach (Waypoint* p, waypointsEditable) { if (p->isNavigationType()) { @@ -578,10 +631,10 @@ int UASWaypointManager::getNavTypeIndexOf(Waypoint* wp) int UASWaypointManager::getGlobalFrameCount() { - // Search through all waypoints, + // Search through all waypointsEditable, // counting only those in global frame int i = 0; - foreach (Waypoint* p, waypoints) + foreach (Waypoint* p, waypointsEditable) { if (p->getFrame() == MAV_FRAME_GLOBAL || p->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) { @@ -594,10 +647,10 @@ int UASWaypointManager::getGlobalFrameCount() int UASWaypointManager::getGlobalFrameAndNavTypeCount() { - // Search through all waypoints, + // Search through all waypointsEditable, // counting only those in global frame int i = 0; - foreach (Waypoint* p, waypoints) { + foreach (Waypoint* p, waypointsEditable) { if ((p->getFrame() == MAV_FRAME_GLOBAL || p->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT) && p->isNavigationType()) { i++; @@ -609,10 +662,10 @@ int UASWaypointManager::getGlobalFrameAndNavTypeCount() int UASWaypointManager::getNavTypeCount() { - // Search through all waypoints, + // Search through all waypointsEditable, // counting only those in global frame int i = 0; - foreach (Waypoint* p, waypoints) { + foreach (Waypoint* p, waypointsEditable) { if (p->isNavigationType()) { i++; } @@ -623,10 +676,10 @@ int UASWaypointManager::getNavTypeCount() int UASWaypointManager::getLocalFrameCount() { - // Search through all waypoints, + // Search through all waypointsEditable, // counting only those in global frame int i = 0; - foreach (Waypoint* p, waypoints) + foreach (Waypoint* p, waypointsEditable) { if (p->getFrame() == MAV_FRAME_LOCAL_NED || p->getFrame() == MAV_FRAME_LOCAL_ENU) { @@ -639,10 +692,10 @@ int UASWaypointManager::getLocalFrameCount() int UASWaypointManager::getLocalFrameIndexOf(Waypoint* wp) { - // Search through all waypoints, + // Search through all waypointsEditable, // counting only those in local frame int i = 0; - foreach (Waypoint* p, waypoints) + foreach (Waypoint* p, waypointsEditable) { if (p->getFrame() == MAV_FRAME_LOCAL_NED || p->getFrame() == MAV_FRAME_LOCAL_ENU) { @@ -659,10 +712,10 @@ int UASWaypointManager::getLocalFrameIndexOf(Waypoint* wp) int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp) { - // Search through all waypoints, + // Search through all waypointsEditable, // counting only those in mission frame int i = 0; - foreach (Waypoint* p, waypoints) + foreach (Waypoint* p, waypointsEditable) { if (p->getFrame() == MAV_FRAME_MISSION) { @@ -677,14 +730,28 @@ int UASWaypointManager::getMissionFrameIndexOf(Waypoint* wp) return -1; } -void UASWaypointManager::readWaypoints() + +/** + * @param readToEdit If true, incoming waypoints will be copied both to "edit"-tab and "view"-tab. Otherwise, only to "view"-tab. + */ +void UASWaypointManager::readWaypoints(bool readToEdit) { + read_to_edit = readToEdit; emit readGlobalWPFromUAS(true); if(current_state == WP_IDLE) { - while(waypoints.size()>0) { - delete waypoints.back(); - waypoints.pop_back(); + //Clear the old view-list before receiving the new one + while(waypointsViewOnly.size()>0) { + delete waypointsViewOnly.back(); + waypointsViewOnly.pop_back(); + } + + //Clear the old edit-list before receiving the new one + if (read_to_edit == true){ + while(waypointsEditable.size()>0) { + delete waypointsEditable.back(); + waypointsEditable.pop_back(); + } } protocol_timer.start(PROTOCOL_TIMEOUT_MS); @@ -704,11 +771,11 @@ void UASWaypointManager::writeWaypoints() { if (current_state == WP_IDLE) { // Send clear all if count == 0 - if (waypoints.count() > 0) { + if (waypointsEditable.count() > 0) { protocol_timer.start(PROTOCOL_TIMEOUT_MS); current_retries = PROTOCOL_MAX_RETRIES; - current_count = waypoints.count(); + current_count = waypointsEditable.count(); current_state = WP_SENDLIST; current_wp_id = 0; current_partner_systemid = uas.getUASID(); @@ -730,7 +797,7 @@ void UASWaypointManager::writeWaypoints() waypoint_buffer.push_back(new mavlink_mission_item_t); mavlink_mission_item_t *cur_d = waypoint_buffer.back(); memset(cur_d, 0, sizeof(mavlink_mission_item_t)); //initialize with zeros - const Waypoint *cur_s = waypoints.at(i); + const Waypoint *cur_s = waypointsEditable.at(i); cur_d->autocontinue = cur_s->getAutoContinue(); cur_d->current = cur_s->getCurrent() & noCurrent; //make sure only one current waypoint is selected, the first selected will be chosen @@ -757,11 +824,11 @@ void UASWaypointManager::writeWaypoints() //send the waypoint count to UAS (this starts the send transaction) sendWaypointCount(); } - } else if (waypoints.count() == 0) { + } else if (waypointsEditable.count() == 0) { sendWaypointClearAll(); } else { //we're in another transaction, ignore command - // // qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command"; + qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command"; } } diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 32945f20946c12173b52400420e9c6aacf562943..6f35996cb6a4ba0e207e52fc83d15d2f2b0350c7 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -79,15 +79,20 @@ public: /** @name Remote operations */ /*@{*/ void clearWaypointList(); ///< Sends the waypoint clear all message to the MAV - void readWaypoints(); ///< Requests the MAV's current waypoint list + + void readWaypoints(bool read_to_edit=false); ///< Requests the MAV's current waypoint list. 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 + int setCurrentWaypoint(quint16 seq); ///< Sends the sequence number of the waypoint that should get the new target waypoint to the UAS + int setCurrentEditable(quint16 seq); ///< Changes the current waypoint in edit tab /*@}*/ /** @name Waypoint list operations */ /*@{*/ - const QVector &getWaypointList(void) { - return waypoints; ///< Returns a const reference to the waypoint list. + const QVector &getWaypointEditableList(void) { + return waypointsEditable; ///< Returns a const reference to the waypoint list. + } + const QVector &getWaypointViewOnlyList(void) { + return waypointsViewOnly; ///< Returns a const reference to the waypoint list. } const QVector getGlobalFrameWaypointList(); ///< Returns a global waypoint list const QVector getGlobalFrameAndNavTypeWaypointList(); ///< Returns a global waypoint list containing only waypoints suitable for navigation. Actions and other mission items are filtered out. @@ -124,21 +129,26 @@ public slots: void timeout(); ///< Called by the timer if a response times out. Handles send retries. /** @name Waypoint list operations */ /*@{*/ - void addWaypoint(Waypoint *wp, bool enforceFirstActive=true); ///< adds a new waypoint to the end of the list and changes its sequence number accordingly + void addWaypointEditable(Waypoint *wp, bool enforceFirstActive=true); ///< adds a new waypoint to the end of the editable list and changes its sequence number accordingly + void addWaypointViewOnly(Waypoint *wp); ///< adds a new waypoint to the end of the view-only list and changes its sequence number accordingly Waypoint* createWaypoint(bool enforceFirstActive=true); ///< Creates a waypoint 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 - void notifyOfChange(Waypoint* wp); ///< Notifies manager to changes to a waypoint + void notifyOfChangeEditable(Waypoint* wp); ///< Notifies manager to changes to an editable waypoint + void notifyOfChangeViewOnly(Waypoint* wp); ///< Notifies manager to changes to a viewonly waypoint, e.g. some widget wants to change "current" /*@}*/ void handleLocalPositionChanged(UASInterface* mav, double x, double y, double z, quint64 time); void handleGlobalPositionChanged(UASInterface* mav, double lat, double lon, double alt, quint64 time); signals: - void waypointListChanged(void); ///< emits signal that the waypoint list has been changed - void waypointListChanged(int uasid); ///< Emits signal that list has been changed - void waypointChanged(int uasid, Waypoint* wp); ///< emits signal that waypoint has been changed + void waypointEditableListChanged(void); ///< emits signal that the list of editable waypoints has been changed + void waypointEditableListChanged(int uasid); ///< emits signal that the list of editable waypoints has been changed + void waypointEditableChanged(int uasid, Waypoint* wp); ///< emits signal that a single editable waypoint has been changed + void waypointViewOnlyListChanged(void); ///< emits signal that the list of editable waypoints has been changed + void waypointViewOnlyListChanged(int uasid); ///< emits signal that the list of editable waypoints has been changed + void waypointViewOnlyChanged(int uasid, Waypoint* wp); ///< emits signal that a single editable waypoint has been changed void currentWaypointChanged(quint16); ///< emits the new current waypoint sequence number void updateStatusString(const QString &); ///< emits the current status string void waypointDistanceChanged(double distance); ///< Distance to next waypoint changed (in meters) @@ -154,9 +164,11 @@ private: WaypointState current_state; ///< The current protocol state quint8 current_partner_systemid; ///< The current protocol communication target system quint8 current_partner_compid; ///< The current protocol communication target component + bool read_to_edit; ///< If true, after readWaypoints() incoming waypoints will be copied both to "edit"-tab and "view"-tab. Otherwise, only to "view"-tab. - QVector waypoints; ///< local waypoint list (main storage) - Waypoint* currentWaypoint; ///< The currently used waypoint + QVector waypointsViewOnly; ///< local copy of current waypoint list on MAV + QVector waypointsEditable; ///< local editable waypoint list + Waypoint* currentWaypointEditable; ///< The currently used waypoint QVector waypoint_buffer; ///< buffer for waypoints during communication QTimer protocol_timer; ///< Timer to catch timeouts }; diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index ad6907e6e8ab0e9b372693155cde30d88bc979d7..f34ae6252d7d1192b7518b49516d1c4b8c3c31c6 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -954,7 +954,7 @@ void HSIDisplay::drawWaypoints(QPainter& painter) { if (uas) { - const QVector& list = uas->getWaypointManager()->getWaypointList(); + const QVector& list = uas->getWaypointManager()->getWaypointEditableList(); QColor color; painter.setBrush(Qt::NoBrush); diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointEditableView.cc similarity index 91% rename from src/ui/WaypointView.cc rename to src/ui/WaypointEditableView.cc index b0ed2cca4ef01a2d6ed90f731f4e485ad2f49ef2..686c01ef382323616bd4f264fc7518a283470f91 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointEditableView.cc @@ -17,15 +17,15 @@ #include #include -#include "WaypointView.h" -#include "ui_WaypointView.h" +#include "WaypointEditableView.h" +#include "ui_WaypointEditableView.h" #include "ui_QGCCustomWaypointAction.h" -WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : +WaypointEditableView::WaypointEditableView(Waypoint* wp, QWidget* parent) : QWidget(parent), customCommand(new Ui_QGCCustomWaypointAction), - viewMode(QGC_WAYPOINTVIEW_MODE_NAV), - m_ui(new Ui::WaypointView) + viewMode(QGC_WAYPOINTEDITABLEVIEW_MODE_NAV), + m_ui(new Ui::WaypointEditableView) { m_ui->setupUi(this); @@ -102,24 +102,24 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : connect(customCommand->param7SpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setParam7(double))); } -void WaypointView::moveUp() +void WaypointEditableView::moveUp() { emit moveUpWaypoint(wp); } -void WaypointView::moveDown() +void WaypointEditableView::moveDown() { emit moveDownWaypoint(wp); } -void WaypointView::remove() +void WaypointEditableView::remove() { emit removeWaypoint(wp); deleteLater(); } -void WaypointView::changedAutoContinue(int state) +void WaypointEditableView::changedAutoContinue(int state) { if (state == 0) wp->setAutocontinue(false); @@ -127,7 +127,7 @@ void WaypointView::changedAutoContinue(int state) wp->setAutocontinue(true); } -void WaypointView::updateActionView(int action) +void WaypointEditableView::updateActionView(int action) { // Remove stretch item at index 17 (m_ui->removeSpacer) m_ui->horizontalLayout->takeAt(17); @@ -231,7 +231,7 @@ void WaypointView::updateActionView(int action) /** * @param index The index of the combo box of the action entry, NOT the action ID */ -void WaypointView::changedAction(int index) +void WaypointEditableView::changedAction(int index) { // set waypoint action int actionIndex = m_ui->comboBox_action->itemData(index).toUInt(); @@ -252,7 +252,7 @@ void WaypointView::changedAction(int index) case MAV_CMD_NAV_LOITER_UNLIM: case MAV_CMD_NAV_LOITER_TURNS: case MAV_CMD_NAV_LOITER_TIME: - changeViewMode(QGC_WAYPOINTVIEW_MODE_NAV); + changeViewMode(QGC_WAYPOINTEDITABLEVIEW_MODE_NAV); // Update frame view updateFrameView(m_ui->comboBox_frame->currentIndex()); // Update view @@ -261,23 +261,23 @@ void WaypointView::changedAction(int index) case MAV_CMD_ENUM_END: default: // Switch to mission frame - changeViewMode(QGC_WAYPOINTVIEW_MODE_DIRECT_EDITING); + changeViewMode(QGC_WAYPOINTEDITABLEVIEW_MODE_DIRECT_EDITING); break; } } -void WaypointView::changeViewMode(QGC_WAYPOINTVIEW_MODE mode) +void WaypointEditableView::changeViewMode(QGC_WAYPOINTEDITABLEVIEW_MODE mode) { viewMode = mode; switch (mode) { - case QGC_WAYPOINTVIEW_MODE_NAV: - case QGC_WAYPOINTVIEW_MODE_CONDITION: + case QGC_WAYPOINTEDITABLEVIEW_MODE_NAV: + case QGC_WAYPOINTEDITABLEVIEW_MODE_CONDITION: // Hide everything, show condition widget // TODO - case QGC_WAYPOINTVIEW_MODE_DO: + case QGC_WAYPOINTEDITABLEVIEW_MODE_DO: break; - case QGC_WAYPOINTVIEW_MODE_DIRECT_EDITING: + case QGC_WAYPOINTEDITABLEVIEW_MODE_DIRECT_EDITING: // Hide almost everything m_ui->orbitSpinBox->hide(); m_ui->takeOffAngleSpinBox->hide(); @@ -307,7 +307,7 @@ void WaypointView::changeViewMode(QGC_WAYPOINTVIEW_MODE mode) } -void WaypointView::updateFrameView(int frame) +void WaypointEditableView::updateFrameView(int frame) { switch(frame) { case MAV_FRAME_GLOBAL: @@ -338,7 +338,7 @@ void WaypointView::updateFrameView(int frame) } } -void WaypointView::deleted(QObject* waypoint) +void WaypointEditableView::deleted(QObject* waypoint) { Q_UNUSED(waypoint); // if (waypoint == this->wp) @@ -347,7 +347,7 @@ void WaypointView::deleted(QObject* waypoint) // } } -void WaypointView::changedFrame(int index) +void WaypointEditableView::changedFrame(int index) { // set waypoint action MAV_FRAME frame = (MAV_FRAME)m_ui->comboBox_frame->itemData(index).toUInt(); @@ -356,19 +356,32 @@ void WaypointView::changedFrame(int index) updateFrameView(frame); } -void WaypointView::changedCurrent(int state) +void WaypointEditableView::changedCurrent(int state) { - if (state == 0) { - m_ui->selectedBox->setChecked(true); - m_ui->selectedBox->setCheckState(Qt::Checked); - wp->setCurrent(false); - } else { + m_ui->selectedBox->blockSignals(true); + if (state == 0) + { + if (wp->getCurrent() == true) //User clicked on the waypoint, that is already current + { + m_ui->selectedBox->setChecked(true); + m_ui->selectedBox->setCheckState(Qt::Checked); + } + else + { + m_ui->selectedBox->setChecked(false); + m_ui->selectedBox->setCheckState(Qt::Unchecked); + wp->setCurrent(false); + } + } + else + { wp->setCurrent(true); - emit changeCurrentWaypoint(wp->getId()); //the slot changeCurrentWaypoint() in WaypointList sets all other current flags to false + //emit changeCurrentWaypoint(wp->getId()); //the slot changeCurrentWaypoint() in WaypointList sets all other current flags to false } + m_ui->selectedBox->blockSignals(false); } -void WaypointView::updateValues() +void WaypointEditableView::updateValues() { // Check if we just lost the wp, delete the widget // accordingly @@ -501,11 +514,11 @@ void WaypointView::updateValues() // If action is unknown, set direct editing mode if (wp->getAction() < 0 || wp->getAction() > MAV_CMD_NAV_TAKEOFF) { - changeViewMode(QGC_WAYPOINTVIEW_MODE_DIRECT_EDITING); + changeViewMode(QGC_WAYPOINTEDITABLEVIEW_MODE_DIRECT_EDITING); } else { - if (viewMode != QGC_WAYPOINTVIEW_MODE_DIRECT_EDITING) + if (viewMode != QGC_WAYPOINTEDITABLEVIEW_MODE_DIRECT_EDITING) { // Action ID known, update m_ui->comboBox_action->setCurrentIndex(action_index); @@ -696,19 +709,19 @@ void WaypointView::updateValues() // wp->blockSignals(false); } -void WaypointView::setCurrent(bool state) +void WaypointEditableView::setCurrent(bool state) { m_ui->selectedBox->blockSignals(true); m_ui->selectedBox->setChecked(state); m_ui->selectedBox->blockSignals(false); } -WaypointView::~WaypointView() +WaypointEditableView::~WaypointEditableView() { delete m_ui; } -void WaypointView::changeEvent(QEvent *e) +void WaypointEditableView::changeEvent(QEvent *e) { switch (e->type()) { case QEvent::LanguageChange: diff --git a/src/ui/WaypointView.h b/src/ui/WaypointEditableView.h similarity index 75% rename from src/ui/WaypointView.h rename to src/ui/WaypointEditableView.h index 21de9247fa3e43eef347910a4b768e9190931045..d2b6c3322b85251f5e788347d76ab51ede36c4b4 100644 --- a/src/ui/WaypointView.h +++ b/src/ui/WaypointEditableView.h @@ -30,33 +30,33 @@ This file is part of the QGROUNDCONTROL project * */ -#ifndef WAYPOINTVIEW_H -#define WAYPOINTVIEW_H +#ifndef WAYPOINTEDITABLEVIEW_H +#define WAYPOINTEDITABLEVIEW_H #include #include "Waypoint.h" #include -enum QGC_WAYPOINTVIEW_MODE { - QGC_WAYPOINTVIEW_MODE_NAV, - QGC_WAYPOINTVIEW_MODE_CONDITION, - QGC_WAYPOINTVIEW_MODE_DO, - QGC_WAYPOINTVIEW_MODE_DIRECT_EDITING +enum QGC_WAYPOINTEDITABLEVIEW_MODE { + QGC_WAYPOINTEDITABLEVIEW_MODE_NAV, + QGC_WAYPOINTEDITABLEVIEW_MODE_CONDITION, + QGC_WAYPOINTEDITABLEVIEW_MODE_DO, + QGC_WAYPOINTEDITABLEVIEW_MODE_DIRECT_EDITING }; namespace Ui { -class WaypointView; +class WaypointEditableView; } class Ui_QGCCustomWaypointAction; -class WaypointView : public QWidget +class WaypointEditableView : public QWidget { Q_OBJECT - Q_DISABLE_COPY(WaypointView) + Q_DISABLE_COPY(WaypointEditableView) public: - explicit WaypointView(Waypoint* wp, QWidget* parent); - virtual ~WaypointView(); + explicit WaypointEditableView(Waypoint* wp, QWidget* parent); + virtual ~WaypointEditableView(); public: void setCurrent(bool state); @@ -76,7 +76,7 @@ public slots: void updateValues(void); protected slots: - void changeViewMode(QGC_WAYPOINTVIEW_MODE mode); + void changeViewMode(QGC_WAYPOINTEDITABLEVIEW_MODE mode); protected: virtual void changeEvent(QEvent *e); @@ -84,18 +84,18 @@ protected: // Special widgets extendending the // waypoint view to mission capabilities Ui_QGCCustomWaypointAction* customCommand; - QGC_WAYPOINTVIEW_MODE viewMode; + QGC_WAYPOINTEDITABLEVIEW_MODE viewMode; private: - Ui::WaypointView *m_ui; + Ui::WaypointEditableView *m_ui; signals: void moveUpWaypoint(Waypoint*); void moveDownWaypoint(Waypoint*); void removeWaypoint(Waypoint*); - void currentWaypointChanged(quint16); + //void currentWaypointChanged(quint16); //unused void changeCurrentWaypoint(quint16); void setYaw(double); }; -#endif // WAYPOINTVIEW_H +#endif // WAYPOINTEDITABLEVIEW_H diff --git a/src/ui/WaypointView.ui b/src/ui/WaypointEditableView.ui similarity index 98% rename from src/ui/WaypointView.ui rename to src/ui/WaypointEditableView.ui index 7e19b7f4a67681feede5ba78a8402ac78884d7a1..99c564a07c911bdbd3ac978dc1d32634a66128ae 100644 --- a/src/ui/WaypointView.ui +++ b/src/ui/WaypointEditableView.ui @@ -1,7 +1,7 @@ - WaypointView - + WaypointEditableView + 0 @@ -604,7 +604,7 @@ Time to stay at this position before advancing - + :/images/actions/go-up.svg:/images/actions/go-up.svg @@ -630,7 +630,7 @@ Time to stay at this position before advancing - + :/images/actions/go-down.svg:/images/actions/go-down.svg @@ -653,7 +653,7 @@ Time to stay at this position before advancing - + :/images/actions/list-remove.svg:/images/actions/list-remove.svg @@ -664,7 +664,7 @@ Time to stay at this position before advancing - + diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index b2ff8cd89159bb771775139684d0559ab4399471..237e161beb2a721bd24cc1895e8a1ee59205bbc6 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -51,16 +51,18 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : { m_ui->setupUi(this); - listLayout = new QVBoxLayout(m_ui->listWidget); - listLayout->setSpacing(0); - listLayout->setMargin(0); - listLayout->setAlignment(Qt::AlignTop); - m_ui->listWidget->setLayout(listLayout); + //EDIT TAB + + editableListLayout = new QVBoxLayout(m_ui->editableListWidget); + editableListLayout->setSpacing(0); + editableListLayout->setMargin(0); + editableListLayout->setAlignment(Qt::AlignTop); + m_ui->editableListWidget->setLayout(editableListLayout); // ADD WAYPOINT // Connect add action, set right button icon and connect action to this class connect(m_ui->addButton, SIGNAL(clicked()), m_ui->actionAddWaypoint, SIGNAL(triggered())); - connect(m_ui->actionAddWaypoint, SIGNAL(triggered()), this, SLOT(add())); + connect(m_ui->actionAddWaypoint, SIGNAL(triggered()), this, SLOT(addEditable())); // ADD WAYPOINT AT CURRENT POSITION connect(m_ui->positionAddButton, SIGNAL(clicked()), this, SLOT(addCurrentPositionWaypoint())); @@ -77,6 +79,17 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : //connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); + //VIEW TAB + + viewOnlyListLayout = new QVBoxLayout(m_ui->viewOnlyListWidget); + viewOnlyListLayout->setSpacing(0); + viewOnlyListLayout->setMargin(0); + viewOnlyListLayout->setAlignment(Qt::AlignTop); + m_ui->viewOnlyListWidget->setLayout(viewOnlyListLayout); + + // REFRESH VIEW TAB + + connect(m_ui->refreshButton, SIGNAL(clicked()), this, SLOT(refresh())); // SET UAS AFTER ALL SIGNALS/SLOTS ARE CONNECTED @@ -126,9 +139,11 @@ void WaypointList::setUAS(UASInterface* uas) connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); connect(uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &))); - connect(uas->getWaypointManager(), SIGNAL(waypointListChanged(void)), this, SLOT(waypointListChanged(void))); - connect(uas->getWaypointManager(), SIGNAL(waypointChanged(int,Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); - connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); + connect(uas->getWaypointManager(), SIGNAL(waypointEditableListChanged(void)), this, SLOT(waypointEditableListChanged(void))); + connect(uas->getWaypointManager(), SIGNAL(waypointEditableChanged(int,Waypoint*)), this, SLOT(updateWaypointEditable(int,Waypoint*))); + connect(uas->getWaypointManager(), SIGNAL(waypointViewOnlyListChanged(void)), this, SLOT(waypointViewOnlyListChanged(void))); + connect(uas->getWaypointManager(), SIGNAL(waypointViewOnlyChanged(int,Waypoint*)), this, SLOT(updateWaypointViewOnly(int,Waypoint*))); + connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointViewOnlyChanged(quint16))); //connect(uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP())); //connect(uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool))); } @@ -164,15 +179,46 @@ void WaypointList::read() { if (uas) { - uas->getWaypointManager()->readWaypoints(); + uas->getWaypointManager()->readWaypoints(true); + } +} + +void WaypointList::refresh() +{ + if (uas) + { + uas->getWaypointManager()->readWaypoints(false); + } +} + +void WaypointList::addEditable() +{ + if (uas) + { + const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); + Waypoint *wp; + if (waypoints.size() > 0) + { + // Create waypoint with last frame + Waypoint *last = waypoints.at(waypoints.size()-1); + wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getParam1(), last->getParam2(), last->getParam3(), last->getParam4(), + last->getAutoContinue(), false, last->getFrame(), last->getAction()); + uas->getWaypointManager()->addWaypointEditable(wp); + } + else + { + // Create first waypoint at current MAV position + addCurrentPositionWaypoint(); + } } } -void WaypointList::add() +/* +void WaypointList::addViewOnly() { if (uas) { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + const QVector &waypoints = uas->getWaypointManager()->getWaypointViewOnlyList(); Waypoint *wp; if (waypoints.size() > 0) { @@ -180,7 +226,7 @@ void WaypointList::add() Waypoint *last = waypoints.at(waypoints.size()-1); wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getParam1(), last->getParam2(), last->getParam3(), last->getParam4(), last->getAutoContinue(), false, last->getFrame(), last->getAction()); - uas->getWaypointManager()->addWaypoint(wp); + uas->getWaypointManager()->addWaypointEditable(wp); } else { @@ -189,12 +235,13 @@ void WaypointList::add() } } } +*/ void WaypointList::addCurrentPositionWaypoint() { if (uas) { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); Waypoint *wp; Waypoint *last = 0; if (waypoints.size() > 0) @@ -215,7 +262,7 @@ void WaypointList::addCurrentPositionWaypoint() } // Create global frame waypoint per default wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, acceptanceRadiusGlobal, holdTime, yawGlobal, true, true, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT); - uas->getWaypointManager()->addWaypoint(wp); + uas->getWaypointManager()->addWaypointEditable(wp); updateStatusLabel(tr("Added GLOBAL, ALTITUDE OVER GROUND waypoint")); } else if (uas->localPositionKnown()) @@ -229,7 +276,7 @@ void WaypointList::addCurrentPositionWaypoint() } // Create local frame waypoint as second option wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), uas->getYaw(), acceptanceRadiusLocal, holdTime, 0.0, true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); - uas->getWaypointManager()->addWaypoint(wp); + uas->getWaypointManager()->addWaypointEditable(wp); updateStatusLabel(tr("Added LOCAL (NED) waypoint")); } else @@ -253,17 +300,43 @@ void WaypointList::changeCurrentWaypoint(quint16 seq) } } -void WaypointList::currentWaypointChanged(quint16 seq) + +void WaypointList::currentWaypointEditableChanged(quint16 seq) +{ + if (uas) + { + const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); + + if (seq < waypoints.size()) + { + for(int i = 0; i < waypoints.size(); i++) + { + WaypointEditableView* widget = wpEditableViews.find(waypoints[i]).value(); + + if (waypoints[i]->getId() == seq) + { + widget->setCurrent(true); + } + else + { + widget->setCurrent(false); + } + } + } + } +} + +void WaypointList::currentWaypointViewOnlyChanged(quint16 seq) { if (uas) { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + const QVector &waypoints = uas->getWaypointManager()->getWaypointViewOnlyList(); if (seq < waypoints.size()) { for(int i = 0; i < waypoints.size(); i++) { - WaypointView* widget = wpViews.find(waypoints[i]).value(); + WaypointViewOnlyView* widget = wpViewOnlyViews.find(waypoints[i]).value(); if (waypoints[i]->getId() == seq) { @@ -278,22 +351,82 @@ void WaypointList::currentWaypointChanged(quint16 seq) } } -void WaypointList::updateWaypoint(int uas, Waypoint* wp) +void WaypointList::updateWaypointEditable(int uas, Waypoint* wp) +{ + Q_UNUSED(uas); + WaypointEditableView *wpv = wpEditableViews.value(wp); + wpv->updateValues(); +} + +void WaypointList::updateWaypointViewOnly(int uas, Waypoint* wp) { Q_UNUSED(uas); - WaypointView *wpv = wpViews.value(wp); + WaypointViewOnlyView *wpv = wpViewOnlyViews.value(wp); wpv->updateValues(); } -void WaypointList::waypointListChanged() +void WaypointList::waypointViewOnlyListChanged() +{ + if (uas) { + // Prevent updates to prevent visual flicker + this->setUpdatesEnabled(false); + const QVector &waypoints = uas->getWaypointManager()->getWaypointViewOnlyList(); + + if (!wpViewOnlyViews.empty()) { + QMapIterator viewIt(wpViewOnlyViews); + 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()) { + WaypointViewOnlyView* widget = wpViewOnlyViews.find(cur).value(); + widget->hide(); + viewOnlyListLayout->removeWidget(widget); + wpViewOnlyViews.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 (!wpViewOnlyViews.contains(wp)) { + WaypointViewOnlyView* wpview = new WaypointViewOnlyView(wp, this); + wpViewOnlyViews.insert(wp, wpview); + connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); + viewOnlyListLayout->insertWidget(i, wpview); + } + WaypointViewOnlyView *wpv = wpViewOnlyViews.value(wp); + + //check if ordering has changed + if(viewOnlyListLayout->itemAt(i)->widget() != wpv) { + viewOnlyListLayout->removeWidget(wpv); + viewOnlyListLayout->insertWidget(i, wpv); + } + + wpv->updateValues(); // update the values of the ui elements in the view + } + this->setUpdatesEnabled(true); + loadFileGlobalWP = false; + } +} + + +void WaypointList::waypointEditableListChanged() { if (uas) { // Prevent updates to prevent visual flicker this->setUpdatesEnabled(false); - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); - if (!wpViews.empty()) { - QMapIterator viewIt(wpViews); + if (!wpEditableViews.empty()) { + QMapIterator viewIt(wpEditableViews); viewIt.toFront(); while(viewIt.hasNext()) { viewIt.next(); @@ -305,10 +438,10 @@ void WaypointList::waypointListChanged() } } if (i == waypoints.size()) { - WaypointView* widget = wpViews.find(cur).value(); + WaypointEditableView* widget = wpEditableViews.find(cur).value(); widget->hide(); - listLayout->removeWidget(widget); - wpViews.remove(cur); + editableListLayout->removeWidget(widget); + wpEditableViews.remove(cur); } } } @@ -316,22 +449,22 @@ void WaypointList::waypointListChanged() // 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); + if (!wpEditableViews.contains(wp)) { + WaypointEditableView* wpview = new WaypointEditableView(wp, this); + wpEditableViews.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))); - listLayout->insertWidget(i, wpview); + //connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); commented, because unused + connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypointEditable(quint16))); + editableListLayout->insertWidget(i, wpview); } - WaypointView *wpv = wpViews.value(wp); + WaypointEditableView *wpv = wpEditableViews.value(wp); //check if ordering has changed - if(listLayout->itemAt(i)->widget() != wpv) { - listLayout->removeWidget(wpv); - listLayout->insertWidget(i, wpv); + if(editableListLayout->itemAt(i)->widget() != wpv) { + editableListLayout->removeWidget(wpv); + editableListLayout->insertWidget(i, wpv); } wpv->updateValues(); // update the values of the ui elements in the view @@ -341,38 +474,38 @@ void WaypointList::waypointListChanged() } } -//void WaypointList::waypointListChanged() +//void WaypointList::waypointEditableListChanged() //{ // if (uas) // { // // Prevent updates to prevent visual flicker // this->setUpdatesEnabled(false); // // Get all waypoints -// const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); +// const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); //// // Store the current state, then check which widgets to update //// // and which ones to delete -//// QList oldWaypoints = wpViews.keys(); +//// QList oldWaypoints = wpEditableViews.keys(); //// foreach (Waypoint* wp, waypoints) //// { -//// WaypointView* wpview; +//// WaypointEditableView* wpview; //// // Create any new waypoint -//// if (!wpViews.contains(wp)) +//// if (!wpEditableViews.contains(wp)) //// { -//// wpview = new WaypointView(wp, this); -//// wpViews.insert(wp, wpview); +//// wpview = new WaypointEditableView(wp, this); +//// wpEditableViews.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))); -//// listLayout->addWidget(wpview); +//// editableListLayout->addWidget(wpview); //// } //// else //// { //// // Update existing waypoints -//// wpview = wpViews.value(wp); +//// wpview = wpEditableViews.value(wp); //// } //// // Mark as updated by removing from old list @@ -386,18 +519,18 @@ void WaypointList::waypointListChanged() //// foreach (Waypoint* wp, oldWaypoints) //// { //// // Delete waypoint view and entry in list -//// WaypointView* wpv = wpViews.value(wp); +//// WaypointEditableView* wpv = wpEditableViews.value(wp); //// if (wpv) //// { -//// listLayout->removeWidget(wpv); +//// editableListLayout->removeWidget(wpv); //// delete wpv; //// } -//// wpViews.remove(wp); +//// wpEditableViews.remove(wp); //// } -// if (!wpViews.empty()) +// if (!wpEditableViews.empty()) // { -// QMapIterator viewIt(wpViews); +// QMapIterator viewIt(wpEditableViews); // viewIt.toFront(); // while(viewIt.hasNext()) // { @@ -413,13 +546,13 @@ void WaypointList::waypointListChanged() // } // if (i == waypoints.size()) // { -// WaypointView* widget = wpViews.find(cur).value(); +// WaypointEditableView* widget = wpEditableViews.find(cur).value(); // if (widget) // { // widget->hide(); -// listLayout->removeWidget(widget); +// editableListLayout->removeWidget(widget); // } -// wpViews.remove(cur); +// wpEditableViews.remove(cur); // } // } // } @@ -428,19 +561,19 @@ void WaypointList::waypointListChanged() // for(int i = 0; i < waypoints.size(); i++) // { // Waypoint *wp = waypoints[i]; -// if (!wpViews.contains(wp)) +// if (!wpEditableViews.contains(wp)) // { -// WaypointView* wpview = new WaypointView(wp, this); -// wpViews.insert(wp, wpview); +// WaypointEditableView* wpview = new WaypointEditableView(wp, this); +// wpEditableViews.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); +// WaypointEditableView *wpv = wpEditableViews.value(wp); // wpv->updateValues(); // update the values of the ui elements in the view -// listLayout->addWidget(wpv); +// editableListLayout->addWidget(wpv); // } // this->setUpdatesEnabled(true); @@ -451,7 +584,7 @@ void WaypointList::waypointListChanged() void WaypointList::moveUp(Waypoint* wp) { if (uas) { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); //get the current position of wp in the local storage int i; @@ -470,7 +603,7 @@ void WaypointList::moveUp(Waypoint* wp) void WaypointList::moveDown(Waypoint* wp) { if (uas) { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); //get the current position of wp in the local storage int i; @@ -512,9 +645,9 @@ void WaypointList::on_clearWPListButton_clicked() if (uas) { emit clearPathclicked(); - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); while(!waypoints.isEmpty()) { //for(int i = 0; i <= waypoints.size(); i++) - WaypointView* widget = wpViews.find(waypoints[0]).value(); + WaypointEditableView* widget = wpEditableViews.find(waypoints[0]).value(); widget->remove(); } } else { @@ -531,7 +664,7 @@ void WaypointList::on_clearWPListButton_clicked() //{ // if (uas) // { -// const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); +// const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); // if (waypoints.size() > 0) // { // Waypoint *temp = waypoints.at(indexWP); @@ -563,9 +696,9 @@ void WaypointList::on_clearWPListButton_clicked() void WaypointList::clearWPWidget() { if (uas) { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + const QVector &waypoints = uas->getWaypointManager()->getWaypointEditableList(); while(!waypoints.isEmpty()) { //for(int i = 0; i <= waypoints.size(); i++) - WaypointView* widget = wpViews.find(waypoints[0]).value(); + WaypointEditableView* widget = wpEditableViews.find(waypoints[0]).value(); widget->remove(); } } diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index c63d67d29f75b44e1a0ed3f4ee24a49bf4bd33c4..3af991589ba29b71afcd1b619db3ff57d93ad5a2 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -40,8 +40,8 @@ This file is part of the QGROUNDCONTROL project #include #include "Waypoint.h" #include "UASInterface.h" -#include "WaypointView.h" - +#include "WaypointEditableView.h" +#include "WaypointViewOnlyView.h" namespace Ui { @@ -69,10 +69,14 @@ public slots: void loadWaypoints(); /** @brief Transmit the local waypoint list to the UAS */ void transmit(); - /** @brief Read the remote waypoint list */ + /** @brief Read the remote waypoint list to both tabs */ void read(); - /** @brief Add a waypoint */ - void add(); + /** @brief Read the remote waypoint list to "view"-tab only*/ + void refresh(); + /** @brief Add a waypoint to "edit"-tab */ + void addEditable(); + /** @brief Add a waypoint to "view"-tab */ + // void addViewOnly(); /** @brief Add a waypoint at the current MAV position */ void addCurrentPositionWaypoint(); /** @brief Add a waypoint by mouse click over the map */ @@ -82,12 +86,18 @@ public slots: void updateStatusLabel(const QString &string); /** @brief The user wants to change the current waypoint */ void changeCurrentWaypoint(quint16 seq); - /** @brief The waypoint planner changed the current waypoint */ - void currentWaypointChanged(quint16 seq); - /** @brief The waypoint manager informs that one waypoint was changed */ - void updateWaypoint(int uas, Waypoint* wp); - /** @brief The waypoint manager informs that the waypoint list was changed */ - void waypointListChanged(void); + /** @brief Current waypoint in edit-tab was changed, so the list must be updated (to contain only one waypoint checked as "current") */ + void currentWaypointEditableChanged(quint16 seq); + /** @brief Current waypoint on UAV was changed, update view-tab */ + void currentWaypointViewOnlyChanged(quint16 seq); + /** @brief The waypoint manager informs that one editable waypoint was changed */ + void updateWaypointEditable(int uas, Waypoint* wp); + /** @brief The waypoint manager informs that one viewonly waypoint was changed */ + void updateWaypointViewOnly(int uas, Waypoint* wp); + /** @brief The waypoint manager informs that the editable waypoint list was changed */ + void waypointEditableListChanged(void); + /** @brief The waypoint manager informs that the waypoint list on the MAV was changed */ + void waypointViewOnlyListChanged(void); // /** @brief The MapWidget informs that a waypoint global was changed on the map */ // void waypointGlobalChanged(const QPointF coordinate, const int indexWP); @@ -115,8 +125,10 @@ protected: virtual void changeEvent(QEvent *e); protected: - QMap wpViews; - QVBoxLayout* listLayout; + QMap wpEditableViews; + QMap wpViewOnlyViews; + QVBoxLayout* viewOnlyListLayout; + QVBoxLayout* editableListLayout; UASInterface* uas; double mavX; double mavY; diff --git a/src/ui/WaypointList.ui b/src/ui/WaypointList.ui index 7c9784276f35ca60f1908a8bbb4c25151dd581d4..8791277c3a5fc8817c694164fbeb6a58476e1439 100644 --- a/src/ui/WaypointList.ui +++ b/src/ui/WaypointList.ui @@ -6,8 +6,8 @@ 0 0 - 798 - 218 + 854 + 398 @@ -19,44 +19,266 @@ Form - - - 4 - - - 4 - - - - - true - - - - - 0 - 0 - 786 - 174 - - - - - 0 - - - 4 - - - + + + + + 0 + + + + Tab 1 + + + + + + true + + + + + 0 + 0 + 812 + 271 + + + + + 0 + + + 4 + + + + + Waypoint list. The list is empty until you issue a read command or add waypoints. + + + Waypoint list. The list is empty until you issue a read command or add waypoints. + + + Waypoint list. The list is empty until you issue a read command or add waypoints. + + + + + + + + + + + Save waypoints to a file on the local harddisk. Does not save them on the MAV. + + + Save waypoints to a file on the local harddisk. Does not save them on the MAV. + + + Save waypoints to a file on the local harddisk. Does not save them on the MAV. + + + Save WPs + + + + + + + Load waypoints from a file on the local harddisk. Does not load the waypoints on the MAV. + + + Load waypoints from a file on the local harddisk. Does not load the waypoints on the MAV. + + + Load waypoints from a file on the local harddisk. Does not load the waypoints on the MAV. + + + Load WPs + + + + + + + Qt::Horizontal + + + + 127 + 20 + + + + + + - Waypoint list. The list is empty until you issue a read command or add waypoints. + Set the current vehicle position as new waypoint - Waypoint list. The list is empty until you issue a read command or add waypoints. + Set the current vehicle position as new waypoint - Waypoint list. The list is empty until you issue a read command or add waypoints. + Set the current vehicle position as new waypoint + + + ... + + + + :/images/actions/go-bottom.svg:/images/actions/go-bottom.svg + + + + + + + Add a new waypoint to this list. Transmit via write to the MAV. + + + Add a new waypoint to this list. Transmit via write to the MAV. + + + Add a new waypoint to this list. Transmit via write to the MAV. + + + ... + + + + :/images/actions/list-add.svg:/images/actions/list-add.svg + + + + + + + Delete all waypoints from this list. You have to click write to clear the list on the MAV as well. + + + Delete all waypoints from this list. You have to click write to clear the list on the MAV as well. + + + Delete all waypoints from this list. You have to click write to clear the list on the MAV as well. + + + ... + + + + :/images/actions/process-stop.svg:/images/actions/process-stop.svg + + + + + + + Read all waypoints from the MAV. Clears the list on this computer. + + + Read all waypoints from the MAV. Clears the list on this computer. + + + Read all waypoints from the MAV. Clears the list on this computer. + + + Read + + + + :/images/status/software-update-available.svg:/images/status/software-update-available.svg + + + + + + + Transmit all waypoints on this list to the MAV. + + + Transmit all waypoints on this list to the MAV. + + + Transmit all waypoints on this list to the MAV. + + + Write + + + + :/images/devices/network-wireless.svg:/images/devices/network-wireless.svg + + + + + + + + Tab 2 + + + + + + true + + + + + 0 + 0 + 812 + 271 + + + + true + + + + + + true + + + + + + + + + + + Qt::Horizontal + + + + 714 + 20 + + + + + + + + Read all waypoints from the MAV and display in View tab.. + + + Read all waypoints from the MAV and display in View tab.. + + + Read all waypoints from the MAV and display in View tab. + + + Refresh + + + + :/images/actions/go-jump.svg:/images/actions/go-jump.svg @@ -64,152 +286,7 @@ - - - - Read all waypoints from the MAV. Clears the list on this computer. - - - Read all waypoints from the MAV. Clears the list on this computer. - - - Read all waypoints from the MAV. Clears the list on this computer. - - - Read - - - - :/images/status/software-update-available.svg:/images/status/software-update-available.svg - - - - - - - Transmit all waypoints on this list to the MAV. - - - Transmit all waypoints on this list to the MAV. - - - Transmit all waypoints on this list to the MAV. - - - Write - - - - :/images/devices/network-wireless.svg:/images/devices/network-wireless.svg - - - - - - - Add a new waypoint to this list. Transmit via write to the MAV. - - - Add a new waypoint to this list. Transmit via write to the MAV. - - - Add a new waypoint to this list. Transmit via write to the MAV. - - - ... - - - - :/images/actions/list-add.svg:/images/actions/list-add.svg - - - - - - - Load waypoints from a file on the local harddisk. Does not load the waypoints on the MAV. - - - Load waypoints from a file on the local harddisk. Does not load the waypoints on the MAV. - - - Load waypoints from a file on the local harddisk. Does not load the waypoints on the MAV. - - - Load WPs - - - - - - - Save waypoints to a file on the local harddisk. Does not save them on the MAV. - - - Save waypoints to a file on the local harddisk. Does not save them on the MAV. - - - Save waypoints to a file on the local harddisk. Does not save them on the MAV. - - - Save WPs - - - - - - - Qt::Horizontal - - - - 127 - 20 - - - - - - - - Set the current vehicle position as new waypoint - - - Set the current vehicle position as new waypoint - - - Set the current vehicle position as new waypoint - - - ... - - - - :/images/actions/go-bottom.svg:/images/actions/go-bottom.svg - - - - - - - Delete all waypoints from this list. You have to click write to clear the list on the MAV as well. - - - Delete all waypoints from this list. You have to click write to clear the list on the MAV as well. - - - Delete all waypoints from this list. You have to click write to clear the list on the MAV as well. - - - ... - - - - :/images/actions/process-stop.svg:/images/actions/process-stop.svg - - - - + The current waypoint transmission status diff --git a/src/ui/WaypointViewOnlyView.cc b/src/ui/WaypointViewOnlyView.cc new file mode 100644 index 0000000000000000000000000000000000000000..e94f17234f387b1eb93f9b077a14883964850f61 --- /dev/null +++ b/src/ui/WaypointViewOnlyView.cc @@ -0,0 +1,175 @@ +#include +#include "WaypointViewOnlyView.h" +#include "ui_WaypointViewOnlyView.h" + +WaypointViewOnlyView::WaypointViewOnlyView(Waypoint* wp, QWidget *parent) : + QWidget(parent), + m_ui(new Ui::WaypointViewOnlyView) +{ + m_ui->setupUi(this); + this->wp = wp; + updateValues(); + + connect(m_ui->current, SIGNAL(stateChanged(int)), this, SLOT(changedCurrent(int))); + connect(m_ui->autoContinue, SIGNAL(stateChanged(int)),this, SLOT(changedAutoContinue(int))); +} + +void WaypointViewOnlyView::changedAutoContinue(int state) +{ + bool new_value = false; + if (state != 0) + { + new_value = true; + } + m_ui->autoContinue->blockSignals(true); + m_ui->autoContinue->setChecked(state); + m_ui->autoContinue->blockSignals(false); + wp->setAutocontinue(new_value); + emit changeAutoContinue(wp->getId(),new_value); + } + +void WaypointViewOnlyView::changedCurrent(int state) +{ + qDebug() << "Trof: WaypointViewOnlyView::changedCurrent(" << state << ") ID:" << wp->getId(); + m_ui->current->blockSignals(true); + if (state == 0) + { + /* + + m_ui->current->setStyleSheet(""); + + */ + if (wp->getCurrent() == true) //User clicked on the waypoint, that is already current + { + m_ui->current->setChecked(true); + m_ui->current->setCheckState(Qt::Checked); + } + else + { + m_ui->current->setChecked(false); + m_ui->current->setCheckState(Qt::Unchecked); + wp->setCurrent(false); + } + } + else + { + /* + FIXME: The checkbox should turn gray to indicate, that set_current request has been sent to UAV. It should become blue (checked) after receiving set_current_ack from waypointplanner. + + m_ui->current->setStyleSheet("*::indicator { \ + border: 1px solid #777777; \ + border-radius: 2px; \ + color: #999999; \ + width: 10px; \ + height: 10px; \ + }"); + */ + wp->setCurrent(true); + emit changeCurrentWaypoint(wp->getId()); //the slot changeCurrentWaypoint() in WaypointList sets all other current flags to false + } + m_ui->current->blockSignals(false); +} + +void WaypointViewOnlyView::setCurrent(bool state) +{ + m_ui->current->blockSignals(true); + m_ui->current->setChecked(state); + m_ui->current->blockSignals(false); +} + +void WaypointViewOnlyView::updateValues() +{ + qDebug() << "Trof: WaypointViewOnlyView::updateValues() ID:" << wp->getId(); + // Check if we just lost the wp, delete the widget + // accordingly + if (!wp) + { + deleteLater(); + return; + } + // Deactivate signals from the WP + wp->blockSignals(true); + // update frame + + m_ui->idLabel->setText(QString("%1").arg(wp->getId())); + + if (m_ui->current->isChecked() != wp->getCurrent()) + { + m_ui->current->setChecked(wp->getCurrent()); + } + if (m_ui->autoContinue->isChecked() != wp->getAutoContinue()) + { + m_ui->autoContinue->setChecked(wp->getAutoContinue()); + } + +switch (wp->getAction()) +{ +case MAV_CMD_NAV_WAYPOINT: + { + if (wp->getParam1()>0) + { + m_ui->displayBar->setText(QString("Go to (%1, %2, %3) and wait there for %4 sec; yaw: %5; rad: %6").arg(wp->getX()).arg(wp->getY()).arg(wp->getZ()).arg(wp->getParam1()).arg(wp->getParam4()).arg(wp->getParam2())); + } + else + { + m_ui->displayBar->setText(QString("Go to (%1, %2, %3); yaw: %4; rad: %5").arg(wp->getX()).arg(wp->getY()).arg(wp->getZ()).arg(wp->getParam4()).arg(wp->getParam2())); + } + break; + } +case MAV_CMD_NAV_LAND: + { + m_ui->displayBar->setText(QString("LAND. Go to (%1, %2, %3) and descent; yaw: %4").arg(wp->getX()).arg(wp->getY()).arg(wp->getZ()).arg(wp->getParam4())); + break; + } +case MAV_CMD_NAV_TAKEOFF: + { + m_ui->displayBar->setText(QString("TAKEOFF. Go to (%1, %2, %3); yaw: %4").arg(wp->getX()).arg(wp->getY()).arg(wp->getZ()).arg(wp->getParam4())); + break; + } +case MAV_CMD_DO_JUMP: + { + if (wp->getParam2()>0) + { + m_ui->displayBar->setText(QString("Jump to waypoint %1. Jumps left: %2").arg(wp->getParam1()).arg(wp->getParam2())); + } + else + { + m_ui->displayBar->setText(QString("No jumps left. Proceed to next waypoint.")); + } + break; + } +case MAV_CMD_CONDITION_DELAY: + { + m_ui->displayBar->setText(QString("Delay: %1 sec").arg(wp->getParam1())); + break; + } +case 237: //MAV_CMD_DO_START_SEARCH + { + m_ui->displayBar->setText(QString("Start searching for pattern. Success when got more than %2 detections with confidence %1").arg(wp->getParam1()).arg(wp->getParam2())); + break; + } +case 238: //MAV_CMD_DO_FINISH_SEARCH + { + m_ui->displayBar->setText(QString("Check if search was successful. yes -> jump to %1, no -> jump to %2. Jumps left: %3").arg(wp->getParam1()).arg(wp->getParam2()).arg(wp->getParam3())); + break; + } +case 240: //MAV_CMD_DO_SWEEP + { + m_ui->displayBar->setText(QString("Sweep. Corners: (%1,%2) and (%3,%4); z: %5; scan radius: %6").arg(wp->getParam3()).arg(wp->getParam4()).arg(wp->getParam5()).arg(wp->getParam6()).arg(wp->getParam7()).arg(wp->getParam1())); + break; + } +default: + { + m_ui->displayBar->setText(QString("Unknown Command ID (%1) : %2, %3, %4, %5, %6, %7, %8").arg(wp->getAction()).arg(wp->getParam1()).arg(wp->getParam2()).arg(wp->getParam3()).arg(wp->getParam4()).arg(wp->getParam5()).arg(wp->getParam6()).arg(wp->getParam7())); + break; + } +} + + + wp->blockSignals(false); +} + +WaypointViewOnlyView::~WaypointViewOnlyView() +{ + delete m_ui; +} diff --git a/src/ui/WaypointViewOnlyView.h b/src/ui/WaypointViewOnlyView.h new file mode 100644 index 0000000000000000000000000000000000000000..099a3d8b2a425b2dec44b93161c090e59076c2ad --- /dev/null +++ b/src/ui/WaypointViewOnlyView.h @@ -0,0 +1,37 @@ +#ifndef WAYPOINTVIEWONLYVIEW_H +#define WAYPOINTVIEWONLYVIEW_H + +#include +#include "Waypoint.h" +#include + +namespace Ui { + class WaypointViewOnlyView; +} + +class WaypointViewOnlyView : public QWidget +{ + Q_OBJECT + +public: + explicit WaypointViewOnlyView(Waypoint* wp, QWidget *parent = 0); + ~WaypointViewOnlyView(); + +public slots: +void changedCurrent(int state); +void changedAutoContinue(int state); +void updateValues(void); +void setCurrent(bool state); + +signals: + void changeCurrentWaypoint(quint16); + void changeAutoContinue(quint16, bool); + +protected: +Waypoint* wp; + +private: + Ui::WaypointViewOnlyView *m_ui; +}; + +#endif // WAYPOINTVIEWONLYVIEW_H diff --git a/src/ui/WaypointViewOnlyView.ui b/src/ui/WaypointViewOnlyView.ui new file mode 100644 index 0000000000000000000000000000000000000000..b2c79910e6d0c5dbe6f485bfb47bb9270e292ed2 --- /dev/null +++ b/src/ui/WaypointViewOnlyView.ui @@ -0,0 +1,244 @@ + + + WaypointViewOnlyView + + + + 0 + 0 + 381 + 55 + + + + + 0 + 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; +} + + + + 2 + + + 0 + + + 2 + + + 0 + + + + + true + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + false + + + false + + + + 2 + + + 2 + + + + + Currently executed waypoint + + + Currently executed waypoint + + + + + + + + + + ID + + + + + + + + 0 + 0 + + + + + 16777215 + 31 + + + + + 8 + + + + QFrame::NoFrame + + + QFrame::Sunken + + + + + + + Automatically continue after this waypoint + + + Automatically continue after this waypoint + + + + + + + + + + + + + + diff --git a/src/ui/map/QGCMapWidget.cc b/src/ui/map/QGCMapWidget.cc index bfcb0da683198241fa54600ce652f880141b0bd8..ed1b371c1f3093e301274e64d7cce05ba22021f4 100644 --- a/src/ui/map/QGCMapWidget.cc +++ b/src/ui/map/QGCMapWidget.cc @@ -164,7 +164,7 @@ void QGCMapWidget::mouseDoubleClickEvent(QMouseEvent* event) wp->setLongitude(pos.Lng()); wp->setAltitude(0); // wp->blockSignals(false); - // currWPManager->notifyOfChange(wp); + // currWPManager->notifyOfChangeEditable(wp); } } OPMapWidget::mouseDoubleClickEvent(event); @@ -190,10 +190,10 @@ void QGCMapWidget::activeUASSet(UASInterface* uas) if (currWPManager) { // Disconnect the waypoint manager / data storage from the UI - disconnect(currWPManager, SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int))); - disconnect(currWPManager, SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); - disconnect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypoint(Waypoint*))); - disconnect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChange(Waypoint*))); + disconnect(currWPManager, SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int))); + disconnect(currWPManager, SIGNAL(waypointEditableChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); + disconnect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypointEditable(Waypoint*))); + disconnect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*))); } if (uas) @@ -201,10 +201,10 @@ void QGCMapWidget::activeUASSet(UASInterface* uas) currWPManager = uas->getWaypointManager(); // Connect the waypoint manager / data storage to the UI - connect(currWPManager, SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int))); - connect(currWPManager, SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); - connect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypoint(Waypoint*))); - connect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChange(Waypoint*))); + connect(currWPManager, SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int))); + connect(currWPManager, SIGNAL(waypointEditableChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); + connect(this, SIGNAL(waypointCreated(Waypoint*)), currWPManager, SLOT(addWaypointEditable(Waypoint*))); + connect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*))); updateSelectedSystem(uas->getUASID()); followUAVID = uas->getUASID(); diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index 009e962cb584c5a4f3bd013ea2abeab873256b67..743ef330c2e4ff6fc15f7ad306d5309934e8cf1b 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -278,7 +278,7 @@ Pixhawk3DWidget::insertWaypoint(void) if (wp) { wp->setFrame(frame); - uas->getWaypointManager()->addWaypoint(wp); + uas->getWaypointManager()->addWaypointEditable(wp); } } } @@ -294,7 +294,7 @@ Pixhawk3DWidget::setWaypoint(void) { if (uas) { const QVector waypoints = - uas->getWaypointManager()->getWaypointList(); + uas->getWaypointManager()->getWaypointEditableList(); Waypoint* waypoint = waypoints.at(selectedWpIndex); if (frame == MAV_FRAME_GLOBAL) { @@ -341,7 +341,7 @@ Pixhawk3DWidget::setWaypointAltitude(void) if (uas) { bool ok; const QVector waypoints = - uas->getWaypointManager()->getWaypointList(); + uas->getWaypointManager()->getWaypointEditableList(); Waypoint* waypoint = waypoints.at(selectedWpIndex); double altitude = waypoint->getZ(); @@ -367,7 +367,7 @@ Pixhawk3DWidget::clearAllWaypoints(void) { if (uas) { const QVector waypoints = - uas->getWaypointManager()->getWaypointList(); + uas->getWaypointManager()->getWaypointEditableList(); for (int i = waypoints.size() - 1; i >= 0; --i) { uas->getWaypointManager()->removeWaypoint(i); } diff --git a/src/ui/map3D/QGCGoogleEarthView.cc b/src/ui/map3D/QGCGoogleEarthView.cc index 332fefbb11e231fddcadde8a022744fb1b372ec9..a05dc0bb9988992403592876186d2401d08495f0 100644 --- a/src/ui/map3D/QGCGoogleEarthView.cc +++ b/src/ui/map3D/QGCGoogleEarthView.cc @@ -209,9 +209,9 @@ void QGCGoogleEarthView::addUAS(UASInterface* uas) connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); // Receive waypoint updates // Connect the waypoint manager / data storage to the UI - connect(uas->getWaypointManager(), SIGNAL(waypointListChanged(int)), this, SLOT(updateWaypointList(int))); - connect(uas->getWaypointManager(), SIGNAL(waypointChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); - //connect(this, SIGNAL(waypointCreated(Waypoint*)), uas->getWaypointManager(), SLOT(addWaypoint(Waypoint*))); + connect(uas->getWaypointManager(), SIGNAL(waypointEditableListChanged(int)), this, SLOT(updateWaypointList(int))); + connect(uas->getWaypointManager(), SIGNAL(waypointEditableChanged(int, Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); + //connect(this, SIGNAL(waypointCreated(Waypoint*)), uas->getWaypointManager(), SLOT(addWaypointEditable(Waypoint*))); // TODO Update waypoint list on UI changes here } @@ -722,7 +722,7 @@ void QGCGoogleEarthView::updateState() wp->setLatitude(latitude); wp->setLongitude(longitude); wp->setAltitude(altitude); - mav->getWaypointManager()->notifyOfChange(wp); + mav->getWaypointManager()->notifyOfChangeEditable(wp); } } } diff --git a/src/ui/map3D/WaypointGroupNode.cc b/src/ui/map3D/WaypointGroupNode.cc index ffacfc42f00317e312ed543c39a22776ec6f6fcf..4d56ac4888a04bbf08b1f56d611d76c2f4fd8a27 100644 --- a/src/ui/map3D/WaypointGroupNode.cc +++ b/src/ui/map3D/WaypointGroupNode.cc @@ -73,7 +73,7 @@ WaypointGroupNode::update(MAV_FRAME frame, UASInterface *uas) removeChild(0, getNumChildren()); } - const QVector& list = uas->getWaypointManager()->getWaypointList(); + const QVector& list = uas->getWaypointManager()->getWaypointEditableList(); for (int i = 0; i < list.size(); i++) { Waypoint* wp = list.at(i);