diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 570103295c4dfee3ffaa7969bfd9418edb6d2483..f4a0b6d49882931bad6a28b98606749f87cb0d6e 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -32,6 +32,7 @@ This file is part of the QGROUNDCONTROL project #include "UASWaypointManager.h" #include "UAS.h" #include "mavlink_types.h" +#include "UASManager.h" #define PROTOCOL_TIMEOUT_MS 2000 ///< maximum time to wait for pending messages until timeout #define PROTOCOL_DELAY_MS 20 ///< minimum delay between sent messages @@ -136,7 +137,7 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui //Clear the old edit-list before receiving the new one if (read_to_edit == true){ - while(waypointsEditable.size()>0) { + while(waypointsEditable.count()>0) { Waypoint *t = waypointsEditable[0]; waypointsEditable.remove(0); delete t; @@ -331,10 +332,10 @@ int UASWaypointManager::setCurrentWaypoint(quint16 seq) int UASWaypointManager::setCurrentEditable(quint16 seq) { - if (seq < waypointsEditable.size()) { + if (seq < waypointsEditable.count()) { if(current_state == WP_IDLE) { //update local main storage - for(int i = 0; i < waypointsEditable.size(); i++) { + for(int i = 0; i < waypointsEditable.count(); i++) { if (waypointsEditable[i]->getId() == seq) { waypointsEditable[i]->setCurrent(true); } else { @@ -370,13 +371,13 @@ void UASWaypointManager::addWaypointEditable(Waypoint *wp, bool enforceFirstActi { if (wp) { - wp->setId(waypointsEditable.size()); - if (enforceFirstActive && waypointsEditable.size() == 0) + wp->setId(waypointsEditable.count()); + if (enforceFirstActive && waypointsEditable.count() == 0) { wp->setCurrent(true); currentWaypointEditable = wp; } - waypointsEditable.insert(waypointsEditable.size(), wp); + waypointsEditable.insert(waypointsEditable.count(), wp); connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*))); emit waypointEditableListChanged(); @@ -390,13 +391,15 @@ void UASWaypointManager::addWaypointEditable(Waypoint *wp, bool enforceFirstActi Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive) { Waypoint* wp = new Waypoint(); - wp->setId(waypointsEditable.size()); - if (enforceFirstActive && waypointsEditable.size() == 0) + wp->setId(waypointsEditable.count()); + wp->setFrame((MAV_FRAME)getFrameRecommendation()); + wp->setAltitude(getAltitudeRecommendation()); + if (enforceFirstActive && waypointsEditable.count() == 0) { wp->setCurrent(true); currentWaypointEditable = wp; } - waypointsEditable.insert(waypointsEditable.size(), wp); + waypointsEditable.append(wp); connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChangeEditable(Waypoint*))); emit waypointEditableListChanged(); @@ -406,17 +409,17 @@ Waypoint* UASWaypointManager::createWaypoint(bool enforceFirstActive) int UASWaypointManager::removeWaypoint(quint16 seq) { - if (seq < waypointsEditable.size()) + if (seq < waypointsEditable.count()) { Waypoint *t = waypointsEditable[seq]; if (t->getCurrent() == true) //trying to remove the current waypoint { - if (seq+1 < waypointsEditable.size()) // setting the next waypoint as current + if (seq+1 < waypointsEditable.count()) // setting the next waypoint as current { waypointsEditable[seq+1]->setCurrent(true); } - else if (seq-1 >= 0) //if deleting the last on the list, then setting the previous waypoint as current + else if (seq-1 >= 0) // if deleting the last on the list, then setting the previous waypoint as current { waypointsEditable[seq-1]->setCurrent(true); } @@ -426,7 +429,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq) delete t; t = NULL; - for(int i = seq; i < waypointsEditable.size(); i++) + for(int i = seq; i < waypointsEditable.count(); i++) { waypointsEditable[i]->setId(i); } @@ -440,7 +443,7 @@ int UASWaypointManager::removeWaypoint(quint16 seq) void UASWaypointManager::moveWaypoint(quint16 cur_seq, quint16 new_seq) { - if (cur_seq != new_seq && cur_seq < waypointsEditable.size() && new_seq < waypointsEditable.size()) + if (cur_seq != new_seq && cur_seq < waypointsEditable.count() && new_seq < waypointsEditable.count()) { Waypoint *t = waypointsEditable[cur_seq]; if (cur_seq < new_seq) { @@ -477,7 +480,7 @@ void UASWaypointManager::saveWaypoints(const QString &saveFile) //write the waypoint list version to the first line for compatibility check out << "QGC WPL 120\r\n"; - for (int i = 0; i < waypointsEditable.size(); i++) + for (int i = 0; i < waypointsEditable.count(); i++) { waypointsEditable[i]->setId(i); waypointsEditable[i]->save(out); @@ -491,7 +494,7 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile) if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) return; - while(waypointsEditable.size()>0) { + while(waypointsEditable.count()>0) { Waypoint *t = waypointsEditable[0]; waypointsEditable.remove(0); delete t; @@ -512,8 +515,8 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile) Waypoint *t = new Waypoint(); if(t->load(in)) { - t->setId(waypointsEditable.size()); - waypointsEditable.insert(waypointsEditable.size(), t); + t->setId(waypointsEditable.count()); + waypointsEditable.insert(waypointsEditable.count(), t); } else { @@ -532,7 +535,7 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile) void UASWaypointManager::clearWaypointList() { - if(current_state == WP_IDLE) + if (current_state == WP_IDLE) { protocol_timer.start(PROTOCOL_TIMEOUT_MS); current_retries = PROTOCOL_MAX_RETRIES; @@ -779,7 +782,7 @@ void UASWaypointManager::readWaypoints(bool readToEdit) /* THIS PART WAS MOVED TO handleWaypointCount. THE EDIT-LIST SHOULD NOT BE CLEARED UNLESS THERE IS A RESPONSE FROM UAV. //Clear the old edit-list before receiving the new one if (read_to_edit == true){ - while(waypointsEditable.size()>0) { + while(waypointsEditable.count()>0) { Waypoint *t = waypointsEditable[0]; waypointsEditable.remove(0); delete t; @@ -1023,3 +1026,39 @@ void UASWaypointManager::sendWaypointAck(quint8 type) uas->sendMessage(message); QGC::SLEEP::msleep(PROTOCOL_DELAY_MS); } + +UAS* UASWaypointManager::getUAS() { + return this->uas; ///< Returns the owning UAS +} + +float UASWaypointManager::getAltitudeRecommendation() +{ + if (waypointsEditable.count() > 0) { + return waypointsEditable.last()->getAltitude(); + } else { + return UASManager::instance()->getHomeAltitude() + getHomeAltitudeOffsetDefault(); + } +} + +int UASWaypointManager::getFrameRecommendation() +{ + if (waypointsEditable.count() > 0) { + return static_cast(waypointsEditable.last()->getFrame()); + } else { + return MAV_FRAME_GLOBAL; + } +} + +float UASWaypointManager::getAcceptanceRadiusRecommendation() +{ + if (waypointsEditable.count() > 0) { + return waypointsEditable.last()->getAcceptanceRadius(); + } else { + return 10.0f; + } +} + +float UASWaypointManager::getHomeAltitudeOffsetDefault() +{ + return defaultAltitudeHomeOffset; +} diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 755de2b9b5da2447e41d01a724e41dbe4294f790..ad0fbbb599e6ac38e43b1d1c8cbfbfac14c2fc4a 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -112,9 +112,11 @@ public: int getLocalFrameCount(); ///< Get the count of local waypoints in the list /*@}*/ - UAS* getUAS() { - return this->uas; ///< Returns the owning UAS - } + UAS* getUAS(); + float getAltitudeRecommendation(); + int getFrameRecommendation(); + float getAcceptanceRadiusRecommendation(); + float getHomeAltitudeOffsetDefault(); private: /** @name Message send functions */ @@ -176,6 +178,9 @@ private: QTimer protocol_timer; ///< Timer to catch timeouts bool standalone; ///< If standalone is set, do not write to UAS quint16 uasid; + + // XXX export to settings + static const float defaultAltitudeHomeOffset = 30.0f; ///< Altitude offset in meters from home for new waypoints }; #endif // UASWAYPOINTMANAGER_H diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index fa4efa4b28a0e8ae898e9041a63e0d582f1f368a..81eef7f06e429ea2fd06f9d33d1dee2c1c514ce4 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -199,8 +199,8 @@ void WaypointList::setUAS(UASInterface* uas) void WaypointList::saveWaypoints() { - QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)")); - WPM->saveWaypoints(fileName); + QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)")); + WPM->saveWaypoints(fileName); } @@ -248,117 +248,180 @@ void WaypointList::refresh() void WaypointList::addEditable() { + addEditable(false); +} - const QVector &waypoints = WPM->getWaypointEditableList(); - Waypoint *wp; - if (waypoints.size() > 0) +void WaypointList::addEditable(bool onCurrentPosition) +{ + + const QVector &waypoints = WPM->getWaypointEditableList(); + Waypoint *wp; + if (waypoints.size() > 0 && + !(onCurrentPosition && uas && (uas->localPositionKnown() || uas->globalPositionKnown()))) + { + // Create waypoint with last frame + Waypoint *last = waypoints.at(waypoints.size()-1); + wp = WPM->createWaypoint(); + wp->blockSignals(true); + MAV_FRAME frame = (MAV_FRAME)last->getFrame(); + wp->setFrame(frame); + if (frame == MAV_FRAME_GLOBAL || frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) { - // 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()); - WPM->addWaypointEditable(wp); + wp->setLatitude(last->getLatitude()); + wp->setLongitude(last->getLongitude()); + wp->setAltitude(last->getAltitude()); + } else { + wp->setX(last->getX()); + wp->setY(last->getY()); + wp->setZ(last->getZ()); } - else - { - if (uas) + wp->setParam1(last->getParam1()); + wp->setParam1(last->getParam2()); + wp->setParam1(last->getParam3()); + wp->setParam1(last->getParam4()); + wp->setAutocontinue(last->getAutoContinue()); + + wp->blockSignals(false); + wp->setAction(last->getAction()); +// WPM->addWaypointEditable(wp); + } + else + { + if (uas) + { + // Create first waypoint at current MAV position + if (uas->globalPositionKnown()) { - // Create first waypoint at current MAV position - if (uas->localPositionKnown() || uas->globalPositionKnown()) + if (onCurrentPosition) { - if(addCurrentPositionWaypoint()) - { - updateStatusLabel(tr("Added default LOCAL (NED) waypoint.")); - wp = new Waypoint(0, 0, 0, -0.50, 0, 0.20, 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); - WPM->addWaypointEditable(wp); - } + + if (WPM->getFrameRecommendation() == MAV_FRAME_GLOBAL) { + updateStatusLabel(tr("Added default GLOBAL (Abs alt.) waypoint.")); + } else { + updateStatusLabel(tr("Added default GLOBAL (Relative alt.) waypoint.")); + } + wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT); + WPM->addWaypointEditable(wp); + + } else { + + if (WPM->getFrameRecommendation() == MAV_FRAME_GLOBAL) { + updateStatusLabel(tr("Added default GLOBAL (Abs alt.) waypoint.")); + } else { + updateStatusLabel(tr("Added default GLOBAL (Relative alt.) waypoint.")); + } + wp = new Waypoint(0, UASManager::instance()->getHomeLatitude(), + UASManager::instance()->getHomeLongitude(), + WPM->getAltitudeRecommendation(), 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT); + WPM->addWaypointEditable(wp); } - else + } + else if (uas->localPositionKnown()) + { + if (onCurrentPosition) { - // MAV connected, but position unknown, add default waypoint - updateStatusLabel(tr("WARNING: No position known. Adding default LOCAL (NED) waypoint")); - wp = new Waypoint(0, 0, 0, -0.50, 0, 0.20, 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); - WPM->addWaypointEditable(wp); + updateStatusLabel(tr("Added default LOCAL (NED) waypoint.")); + wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); + WPM->addWaypointEditable(wp); + } else { + updateStatusLabel(tr("Added default LOCAL (NED) waypoint.")); + wp = new Waypoint(0, 0, 0, -0.50, 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); + WPM->addWaypointEditable(wp); } } else { - //Since no UAV available, create first default waypoint. - updateStatusLabel(tr("No UAV connected. Adding default LOCAL (NED) waypoint")); - wp = new Waypoint(0, 0, 0, -0.50, 0, 0.20, 0, 0,true, true, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); + // MAV connected, but position unknown, add default waypoint + updateStatusLabel(tr("WARNING: No position known. Adding default LOCAL (NED) waypoint")); + wp = new Waypoint(0, UASManager::instance()->getHomeLatitude(), + UASManager::instance()->getHomeLongitude(), + WPM->getAltitudeRecommendation(), 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT); WPM->addWaypointEditable(wp); - //create a popup notifying the user about the limitations of offline editing - if (showOfflineWarning == true) - { - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Warning); - msgBox.setText("Offline editor!"); - msgBox.setInformativeText("You are using the offline mission editor. Please don't forget to save your mission plan before connecting the UAV, otherwise it will be lost."); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - msgBox.exec(); - showOfflineWarning = false; - } } } - -} - - -int WaypointList::addCurrentPositionWaypoint() -{ - if (uas) - { - const QVector &waypoints = WPM->getWaypointEditableList(); - Waypoint *wp; - Waypoint *last = 0; - if (waypoints.size() > 0) - { - last = waypoints.at(waypoints.size()-1); - } - - if (uas->globalPositionKnown()) + else { - float acceptanceRadiusGlobal = 10.0f; - float holdTime = 0.0f; - float yawGlobal = 0.0f; - if (last) - { - acceptanceRadiusGlobal = last->getAcceptanceRadius(); - holdTime = last->getHoldTime(); - yawGlobal = last->getYaw(); - } - // Create global frame waypoint per default - wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, acceptanceRadiusGlobal, holdTime, yawGlobal, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT); + //Since no UAV available, create first default waypoint. + updateStatusLabel(tr("No UAV connected. Adding default GLOBAL (NED) waypoint")); + wp = new Waypoint(0, UASManager::instance()->getHomeLatitude(), + UASManager::instance()->getHomeLongitude(), + WPM->getAltitudeRecommendation(), 0, WPM->getAcceptanceRadiusRecommendation(), 0, 0,true, true, (MAV_FRAME)WPM->getFrameRecommendation(), MAV_CMD_NAV_WAYPOINT); WPM->addWaypointEditable(wp); - updateStatusLabel(tr("Added GLOBAL, ALTITUDE OVER GROUND waypoint")); - return 0; - } - else if (uas->localPositionKnown()) - { - float acceptanceRadiusLocal = 0.2f; - float holdTime = 0.5f; - if (last) + //create a popup notifying the user about the limitations of offline editing + if (showOfflineWarning == true) { - acceptanceRadiusLocal = last->getAcceptanceRadius(); - holdTime = last->getHoldTime(); + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Warning); + msgBox.setText("Offline editor!"); + msgBox.setInformativeText("You are using the offline mission editor. Please don't forget to save your mission plan before connecting the UAV, otherwise it will be lost."); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + showOfflineWarning = false; } - // Create local frame waypoint as second option - wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), uas->getYaw(), acceptanceRadiusLocal, holdTime, 0.0, true, false, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); - WPM->addWaypointEditable(wp); - updateStatusLabel(tr("Added LOCAL (NED) waypoint")); - return 0; - } - else - { - // Do nothing - updateStatusLabel(tr("Not adding waypoint, no position of MAV known yet.")); - return 1; } } - return 1; } +void WaypointList::addCurrentPositionWaypoint() { + addEditable(true); +} + + +//int WaypointList::addCurrentPositionWaypoint() +//{ +// if (uas) +// { +// const QVector &waypoints = WPM->getWaypointEditableList(); +// Waypoint *wp; +// Waypoint *last = 0; +// if (waypoints.size() > 0) +// { +// last = waypoints.at(waypoints.size()-1); +// } + +// if (uas->globalPositionKnown()) +// { +// float acceptanceRadiusGlobal = 10.0f; +// float holdTime = 0.0f; +// float yawGlobal = 0.0f; +// if (last) +// { +// acceptanceRadiusGlobal = last->getAcceptanceRadius(); +// holdTime = last->getHoldTime(); +// yawGlobal = last->getYaw(); +// } +// // Create global frame waypoint per default +// wp = new Waypoint(0, uas->getLatitude(), uas->getLongitude(), uas->getAltitude(), 0, acceptanceRadiusGlobal, holdTime, yawGlobal, true, false, MAV_FRAME_GLOBAL_RELATIVE_ALT, MAV_CMD_NAV_WAYPOINT); +// WPM->addWaypointEditable(wp); +// updateStatusLabel(tr("Added GLOBAL, ALTITUDE OVER GROUND waypoint")); +// return 0; +// } +// else if (uas->localPositionKnown()) +// { +// float acceptanceRadiusLocal = 0.2f; +// float holdTime = 0.5f; +// if (last) +// { +// acceptanceRadiusLocal = last->getAcceptanceRadius(); +// holdTime = last->getHoldTime(); +// } +// // Create local frame waypoint as second option +// wp = new Waypoint(0, uas->getLocalX(), uas->getLocalY(), uas->getLocalZ(), uas->getYaw(), acceptanceRadiusLocal, holdTime, 0.0, true, false, MAV_FRAME_LOCAL_NED, MAV_CMD_NAV_WAYPOINT); +// WPM->addWaypointEditable(wp); +// updateStatusLabel(tr("Added LOCAL (NED) waypoint")); +// return 0; +// } +// else +// { +// // Do nothing +// updateStatusLabel(tr("Not adding waypoint, no position of MAV known yet.")); +// return 1; +// } +// } +// return 1; +//} + void WaypointList::updateStatusLabel(const QString &string) { // Status label in write widget @@ -646,11 +709,17 @@ void WaypointList::on_clearWPListButton_clicked() void WaypointList::clearWPWidget() { + // Get list const QVector &waypoints = WPM->getWaypointEditableList(); + + + // XXX delete wps as well + + // Clear UI elements while(!waypoints.isEmpty()) { //for(int i = 0; i <= waypoints.size(); i++) WaypointEditableView* widget = wpEditableViews.find(waypoints[0]).value(); widget->remove(); - } + } } //void WaypointList::setIsLoadFileWP() diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index 97f80e2f5c5bdc5635904d8de11c61455ec3bc3a..2871105c02cc4b8d73e6c6bbe1087802dcb2a746 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -77,9 +77,11 @@ public slots: /** @brief Read the remote waypoint list to "view"-tab only*/ void refresh(); /** @brief Add a waypoint to "edit"-tab */ - void addEditable(); + void addEditable(); + /** @brief Add a waypoint to "edit"-tab on current MAV position or on generic position (home) */ + void addEditable(bool onCurrentPosition); /** @brief Add a waypoint at the current MAV position */ - int addCurrentPositionWaypoint(); + void addCurrentPositionWaypoint(); /** @brief Add a waypoint by mouse click over the map */ //Update events diff --git a/src/ui/map/QGCMapWidget.cc b/src/ui/map/QGCMapWidget.cc index ad3920d0dcc9aa3a7c3c09e6c56ca1c41eb14751..6b3e8aa40c9927fb665af3addeb18284af31076c 100644 --- a/src/ui/map/QGCMapWidget.cc +++ b/src/ui/map/QGCMapWidget.cc @@ -17,10 +17,10 @@ QGCMapWidget::QGCMapWidget(QWidget *parent) : followUAVID(0), mapInitialized(false), homeAltitude(0), - uas(0) + uas(NULL) { - //currWPManager = new UASWaypointManager(NULL); //Create a waypoint manager that is null. currWPManager = UASManager::instance()->getActiveUASWaypointManager(); + waypointLines.insert(0, new QGraphicsItemGroup(map)); 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*))); @@ -207,6 +207,18 @@ void QGCMapWidget::loadSettings(bool changePosition) trailInterval = settings.value("TRAIL_INTERVAL", trailInterval).toFloat(); settings.endGroup(); + // SET CORRECT MENU CHECKBOXES + // Set the correct trail interval + if (trailType == mapcontrol::UAVTrailType::ByDistance) + { + // XXX +#warning Settings loading for trail type not implemented + } + else if (trailType == mapcontrol::UAVTrailType::ByTimeElapsed) + { + // XXX + } + // SET TRAIL TYPE foreach (mapcontrol::UAVItem* uav, GetUAVS()) { @@ -245,25 +257,22 @@ void QGCMapWidget::storeSettings() void QGCMapWidget::mouseDoubleClickEvent(QMouseEvent* event) { - - // FIXME HACK! - //if (currEditMode == EDIT_MODE_WAYPOINTS) + // If a waypoint manager is available + if (currWPManager) { - // If a waypoint manager is available - if (currWPManager) - { - // Create new waypoint - internals::PointLatLng pos = map->FromLocalToLatLng(event->pos().x(), event->pos().y()); - Waypoint* wp = currWPManager->createWaypoint(); - // wp->blockSignals(true); - // wp->setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT); - wp->setLatitude(pos.Lat()); - wp->setLongitude(pos.Lng()); - wp->setAltitude(0); - // wp->blockSignals(false); - // currWPManager->notifyOfChangeEditable(wp); - } + // Create new waypoint + internals::PointLatLng pos = map->FromLocalToLatLng(event->pos().x(), event->pos().y()); + Waypoint* wp = currWPManager->createWaypoint(); + // wp->blockSignals(true); + // wp->setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT); + wp->setLatitude(pos.Lat()); + wp->setLongitude(pos.Lng()); + wp->setFrame((MAV_FRAME)currWPManager->getFrameRecommendation()); + wp->setAltitude(currWPManager->getAltitudeRecommendation()); + // wp->blockSignals(false); + // currWPManager->notifyOfChangeEditable(wp); } + OPMapWidget::mouseDoubleClickEvent(event); } @@ -276,6 +285,14 @@ void QGCMapWidget::addUAS(UASInterface* uas) { connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(updateSystemSpecs(int))); + if (!waypointLines.value(uas->getUASID(), NULL)) { + waypointLines.insert(uas->getUASID(), new QGraphicsItemGroup(map)); + } else { + foreach (QGraphicsItem* item, waypointLines.value(uas->getUASID())->childItems()) + { + delete item; + } + } } void QGCMapWidget::activeUASSet(UASInterface* uas) @@ -294,45 +311,17 @@ void QGCMapWidget::activeUASSet(UASInterface* uas) disconnect(this, SIGNAL(waypointChanged(Waypoint*)), currWPManager, SLOT(notifyOfChangeEditable(Waypoint*))); } - if (uas) - { - //UASWaypointManager *old = currWPManager; - - currWPManager = uas->getWaypointManager(); - - updateSelectedSystem(uas->getUASID()); - followUAVID = uas->getUASID(); - - updateWaypointList(uas->getUASID()); + currWPManager = uas->getWaypointManager(); - // Connect the waypoint manager / data storage to the UI - 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(); + updateWaypointList(uas->getUASID()); - /*if (offlineMode) - { - - if (QMessageBox::question(0,"Question","Do you want to append the offline waypoints to the ones currently on the UAV?",QMessageBox::Yes,QMessageBox::No) == QMessageBox::Yes) - { - //Need to transfer all waypoints from the offline mode WPManager to the online mode. - for (int i=0;igetWaypointEditableList().size();i++) - { - Waypoint *wp = currWPManager->createWaypoint(); - wp->setLatitude(old->getWaypointEditableList()[i]->getLatitude()); - wp->setLongitude(old->getWaypointEditableList()[i]->getLongitude()); - wp->setAltitude(old->getWaypointEditableList()[i]->getAltitude()); - } - } - offlineMode = false; - old->deleteLater(); - }*/ - - - // Delete all waypoints and add waypoint from new system - - } + // Connect the waypoint manager / data storage to the UI + 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*))); } /** @@ -360,9 +349,17 @@ void QGCMapWidget::updateGlobalPosition(UASInterface* uas, double lat, double lo newUAV->setParentItem(map); UAVS.insert(uas->getUASID(), newUAV); uav = GetUAV(uas->getUASID()); - uav->SetTrailTime(1); - uav->SetTrailDistance(5); - uav->SetTrailType(mapcontrol::UAVTrailType::ByTimeElapsed); + // Set the correct trail type + uav->SetTrailType(trailType); + // Set the correct trail interval + if (trailType == mapcontrol::UAVTrailType::ByDistance) + { + uav->SetTrailDistance(trailInterval); + } + else if (trailType == mapcontrol::UAVTrailType::ByTimeElapsed) + { + uav->SetTrailTime(trailInterval); + } } // Set new lat/lon position of UAV icon @@ -551,6 +548,11 @@ void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint) { // Block circle updates Waypoint* wp = iconsToWaypoints.value(waypoint, NULL); + + // Delete UI element if wp doesn't exist + if (!wp) + WPDelete(waypoint); + // Protect from vicious double update cycle if (firingWaypointChange == wp) return; // Not in cycle, block now from entering it @@ -565,8 +567,8 @@ void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint) wp->setLatitude(pos.Lat()); wp->setLongitude(pos.Lng()); // XXX Magic values - wp->setAltitude(homeAltitude + 50.0f); - wp->setAcceptanceRadius(10.0f); +// wp->setAltitude(homeAltitude + 50.0f); +// wp->setAcceptanceRadius(10.0f); wp->blockSignals(false); @@ -589,13 +591,15 @@ void QGCMapWidget::handleMapWaypointEdit(mapcontrol::WayPointItem* waypoint) */ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp) { - qDebug() << "UPDATING WP FUNCTION CALLED"; + qDebug() << __FILE__ << __LINE__ << "UPDATING WP FUNCTION CALLED"; // Source of the event was in this widget, do nothing - if (firingWaypointChange == wp) return; + if (firingWaypointChange == wp) { + return; + } // Currently only accept waypoint updates from the UAS in focus // this has to be changed to accept read-only updates from other systems as well. UASInterface* uasInstance = UASManager::instance()->getUASForId(uas); - if (!uasInstance || uasInstance->getWaypointManager() == currWPManager || uas == -1) + if (currWPManager) { // Only accept waypoints in global coordinate frame if (((wp->getFrame() == MAV_FRAME_GLOBAL) || (wp->getFrame() == MAV_FRAME_GLOBAL_RELATIVE_ALT)) && wp->isNavigationType()) @@ -607,7 +611,7 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp) // as we're only handling global waypoints int wpindex = currWPManager->getGlobalFrameAndNavTypeIndexOf(wp); // If not found, return (this should never happen, but helps safety) - if (wpindex == -1) return; + if (wpindex < 0) return; // Mark this wp as currently edited firingWaypointChange = wp; @@ -686,7 +690,7 @@ void QGCMapWidget::updateWaypoint(int uas, Waypoint* wp) // waypoint list. This implies that the coordinate frame of this // waypoint was changed and the list containing only global // waypoints was shortened. Thus update the whole list - if (waypointsToIcons.size() > currWPManager->getGlobalFrameAndNavTypeCount()) + if (waypointsToIcons.count() > currWPManager->getGlobalFrameAndNavTypeCount()) { updateWaypointList(uas); } @@ -705,13 +709,23 @@ void QGCMapWidget::updateWaypointList(int uas) // Currently only accept waypoint updates from the UAS in focus // this has to be changed to accept read-only updates from other systems as well. UASInterface* uasInstance = UASManager::instance()->getUASForId(uas); - if ((uasInstance && (uasInstance->getWaypointManager() == currWPManager)) || uas == -1) + if (currWPManager) { // ORDER MATTERS HERE! // TWO LOOPS ARE NEEDED - INFINITY LOOP ELSE qDebug() << "DELETING NOW OLD WPS"; + // Delete connecting waypoint lines + QGraphicsItemGroup* group = waypointLines.value(uas, NULL); + if (group) + { + foreach (QGraphicsItem* item, group->childItems()) + { + delete item; + } + } + // Delete first all old waypoints // this is suboptimal (quadratic, but wps should stay in the sub-100 range anyway) QVector wps = currWPManager->getGlobalFrameAndNavTypeWaypointList(); @@ -742,16 +756,6 @@ void QGCMapWidget::updateWaypointList(int uas) if (!waypointsToIcons.contains(wp)) updateWaypoint(uas, wp); } - // Delete connecting waypoint lines - QGraphicsItemGroup* group = waypointLines.value(uas, NULL); - if (group) - { - foreach (QGraphicsItem* item, group->childItems()) - { - delete item; - } - } - // Add line element if this is NOT the first waypoint mapcontrol::WayPointItem* prevIcon = NULL; foreach (Waypoint* wp, wps)