diff --git a/src/Waypoint.cc b/src/Waypoint.cc index bb351a4..c36ec60 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -55,23 +55,27 @@ Waypoint::~Waypoint() void Waypoint::save(QTextStream &saveStream) { - saveStream << "\t" << this->getId() << "\t" << this->getX() << "\t" << this->getY() << "\t" << this->getZ() << "\t" << this->getYaw() << "\t" << this->getAutoContinue() << "\t" << this->getCurrent() << "\t" << this->getOrbit() << "\t" << this->getHoldTime() << "\n"; + saveStream << this->getId() << "\t" << this->getFrame() << "\t" << this->getAction() << "\t" << this->getOrbit() << "\t" << /*Orbit Direction*/ 0 << "\t" << this->getOrbit() << "\t" << this->getHoldTime() << "\t" << this->getCurrent() << "\t" << this->getX() << "\t" << this->getY() << "\t" << this->getZ() << "\t" << this->getYaw() << "\t" << this->getAutoContinue() << "\r\n"; } bool Waypoint::load(QTextStream &loadStream) { const QStringList &wpParams = loadStream.readLine().split("\t"); - if (wpParams.size() == 10) + if (wpParams.size() == 13) { - this->id = wpParams[1].toInt(); - this->x = wpParams[2].toDouble(); - this->y = wpParams[3].toDouble(); - this->z = wpParams[4].toDouble(); - this->yaw = wpParams[5].toDouble(); - this->autocontinue = (wpParams[6].toInt() == 1 ? true : false); + this->id = wpParams[0].toInt(); + this->frame = (MAV_FRAME) wpParams[1].toInt(); + this->action = (MAV_ACTION) wpParams[2].toInt(); + this->orbit = wpParams[3].toDouble(); + //TODO: orbit direction + //TODO: param1 + this->holdTime = wpParams[6].toInt(); this->current = (wpParams[7].toInt() == 1 ? true : false); - this->orbit = wpParams[8].toDouble(); - this->holdTime = wpParams[9].toInt(); + this->x = wpParams[8].toDouble(); + this->y = wpParams[9].toDouble(); + this->z = wpParams[10].toDouble(); + this->yaw = wpParams[11].toDouble(); + this->autocontinue = (wpParams[12].toInt() == 1 ? true : false); return true; } return false; diff --git a/src/Waypoint.h b/src/Waypoint.h index 6e4f6ae..fa2246d 100644 --- a/src/Waypoint.h +++ b/src/Waypoint.h @@ -44,7 +44,7 @@ class Waypoint : public QObject public: Waypoint(quint16 id = 0, float x = 0.0f, float y = 0.0f, float z = 0.0f, float yaw = 0.0f, bool autocontinue = false, - bool current = false, float orbit = 0.1f, int holdTime = 2000, + bool current = false, float orbit = 0.15f, int holdTime = 0, MAV_FRAME frame=MAV_FRAME_GLOBAL, MAV_ACTION action=MAV_ACTION_NAVIGATE); ~Waypoint(); diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 0c3581e..c1d4298 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -31,6 +31,7 @@ This file is part of the QGROUNDCONTROL project #include "UASWaypointManager.h" #include "UAS.h" +#include "MainWindow.h" #define PROTOCOL_TIMEOUT_MS 2000 ///< maximum time to wait for pending messages until timeout #define PROTOCOL_DELAY_MS 40 ///< minimum delay between sent messages @@ -355,6 +356,10 @@ void UASWaypointManager::saveWaypoints(const QString &saveFile) return; QTextStream out(&file); + + //write the waypoint list version to the first line for compatibility check + out << "QGC WPL 100\r\n"; + for (int i = 0; i < waypoints.size(); i++) { waypoints[i]->save(out); @@ -376,20 +381,35 @@ void UASWaypointManager::loadWaypoints(const QString &loadFile) } QTextStream in(&file); - while (!in.atEnd()) + + const QStringList &version = in.readLine().split(" "); + + if (!(version.size() == 3 && version[0] == "QGC" && version[1] == "WPL" && version[2] == "100")) { - Waypoint *t = new Waypoint(); - if(t->load(in)) + MainWindow::instance()->showCriticalMessage(tr("Error loading waypoint file"),tr("The waypoint file is not compatible with the current version of QGroundControl.")); + } + else + { + while (!in.atEnd()) { - t->setId(waypoints.size()); - waypoints.insert(waypoints.size(), t); + Waypoint *t = new Waypoint(); + if(t->load(in)) + { + t->setId(waypoints.size()); + waypoints.insert(waypoints.size(), t); + } + else + { + MainWindow::instance()->showCriticalMessage(tr("Error loading waypoint file"),tr("The waypoint file is corrupted. Load operation only partly succesful.")); + break; + } } } + file.close(); emit loadWPFile(); emit waypointListChanged(); - } @@ -429,8 +449,7 @@ void UASWaypointManager::readWaypoints() { while(waypoints.size()>0) { - Waypoint *t = waypoints.back(); - delete t; + delete waypoints.back(); waypoints.pop_back(); } @@ -464,7 +483,7 @@ void UASWaypointManager::writeWaypoints() current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER; //clear local buffer - //TODO: Why not replace with waypoint_buffer.clear() ? + //TODO: Why not replace with waypoint_buffer.clear() ? - because this will lead to memory leaks, the waypoint-structs have to be deleted, clear() would only delete the pointers. while(!waypoint_buffer.empty()) { delete waypoint_buffer.back(); @@ -503,6 +522,10 @@ void UASWaypointManager::writeWaypoints() sendWaypointCount(); } } + else if (waypoints.count() == 0) + { + sendWaypointClearAll(); + } else { //we're in another transaction, ignore command diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index b9a090b..f2de576 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -211,8 +211,7 @@ void WaypointList::add() else { // Create global frame waypoint per default - wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), - 0.0, true, true, 0.15, 2000); + wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_ACTION_NAVIGATE); uas->getWaypointManager().addWaypoint(wp); } if (wp->getFrame() == MAV_FRAME_GLOBAL)