From 11fa19c3a25a11dc2dcd33929d2afb39c972d319 Mon Sep 17 00:00:00 2001 From: pixhawk Date: Mon, 16 Aug 2010 17:10:53 +0200 Subject: [PATCH] Major refactoring of the waypoint code. The new design fits the model-view-controller pattern better. --- src/Waypoint.cc | 25 ++++ src/Waypoint.h | 5 + src/uas/UASWaypointManager.cc | 208 ++++++++++++++++++++++---- src/uas/UASWaypointManager.h | 28 +++- src/ui/HSIDisplay.cc | 2 +- src/ui/WaypointList.cc | 273 +++++++++++----------------------- src/ui/WaypointList.h | 14 +- src/ui/WaypointView.cc | 29 ++-- src/ui/WaypointView.h | 1 + 9 files changed, 340 insertions(+), 245 deletions(-) diff --git a/src/Waypoint.cc b/src/Waypoint.cc index b3d7ca4bf..c194214a6 100644 --- a/src/Waypoint.cc +++ b/src/Waypoint.cc @@ -31,6 +31,7 @@ This file is part of the PIXHAWK project */ #include "Waypoint.h" +#include Waypoint::Waypoint(quint16 _id, float _x, float _y, float _z, float _yaw, bool _autocontinue, bool _current, float _orbit, int _holdTime) : id(_id), @@ -50,6 +51,30 @@ 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"; +} + +bool Waypoint::load(QTextStream &loadStream) +{ + const QStringList &wpParams = loadStream.readLine().split("\t"); + if (wpParams.size() == 10) + { + 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->current = (wpParams[7].toInt() == 1 ? true : false); + this->orbit = wpParams[8].toDouble(); + this->holdTime = wpParams[9].toInt(); + return true; + } + return false; +} + void Waypoint::setId(quint16 id) { this->id = id; diff --git a/src/Waypoint.h b/src/Waypoint.h index e5c4ae1bd..053f6cbb0 100644 --- a/src/Waypoint.h +++ b/src/Waypoint.h @@ -35,6 +35,7 @@ This file is part of the PIXHAWK project #include #include +#include class Waypoint : public QObject { @@ -54,6 +55,10 @@ public: float getOrbit() const { return orbit; } float getHoldTime() const { return holdTime; } + void save(QTextStream &saveStream); + bool load(QTextStream &loadStream); + + private: quint16 id; float x; diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 5e7a3d644..71924316a 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -131,8 +131,8 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ if(wp->seq == current_wp_id) { - //update the UI FIXME - emit waypointUpdated(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2); + Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2); + localAddWaypoint(lwp); //get next waypoint current_wp_id++; @@ -223,42 +223,182 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m { protocol_timer.stop(); 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); + } + else + { + waypoints[i]->setCurrent(false); + } + } + } + emit updateStatusString(QString("New current waypoint %1").arg(wpc->seq)); + //emit update to UI widgets + emit currentWaypointChanged(wpc->seq); } qDebug() << "new current waypoint" << wpc->seq; - emit currentWaypointChanged(wpc->seq); } } -void UASWaypointManager::clearWaypointList() +int UASWaypointManager::setCurrentWaypoint(quint16 seq) { - if(current_state == WP_IDLE) + if (seq < waypoints.size()) { - protocol_timer.start(PROTOCOL_TIMEOUT_MS); - current_retries = PROTOCOL_MAX_RETRIES; + 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); + } + else + { + waypoints[i]->setCurrent(false); + } + } - current_state = WP_CLEARLIST; - current_wp_id = 0; - current_partner_systemid = uas.getUASID(); - current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER; + //TODO: signal changed waypoint list - sendWaypointClearAll(); + //send change to UAS - important to note: if the transmission fails, we have inconsistencies + protocol_timer.start(PROTOCOL_TIMEOUT_MS); + current_retries = PROTOCOL_MAX_RETRIES; + + current_state = WP_SETCURRENT; + current_wp_id = seq; + current_partner_systemid = uas.getUASID(); + current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER; + + sendWaypointSetCurrent(current_wp_id); + + //emit waypointListChanged(); + + return 0; + } + } + return -1; +} + +void UASWaypointManager::localAddWaypoint(Waypoint *wp) +{ + if (wp) + { + wp->setId(waypoints.size()); + waypoints.insert(waypoints.size(), wp); + emit waypointListChanged(); } } -void UASWaypointManager::setCurrent(quint16 seq) +int UASWaypointManager::localRemoveWaypoint(quint16 seq) +{ + if (seq < waypoints.size()) + { + Waypoint *t = waypoints[seq]; + waypoints.remove(seq); + delete t; + + for(int i = seq; i < waypoints.size(); i++) + { + waypoints[i]->setId(i); + } + emit waypointListChanged(); + return 0; + } + return -1; +} + +void UASWaypointManager::localMoveWaypoint(quint16 cur_seq, quint16 new_seq) +{ + if (cur_seq != new_seq && cur_seq < waypoints.size() && new_seq < waypoints.size()) + { + Waypoint *t = waypoints[cur_seq]; + if (cur_seq < new_seq) + { + for (int i = cur_seq; i < new_seq; i++) + { + waypoints[i] = waypoints[i+1]; + //waypoints[i]->setId(i); // let the local IDs stay the same so that they can be found more easily by the user + } + } + else + { + for (int i = cur_seq; i > new_seq; i--) + { + waypoints[i] = waypoints[i-1]; + // waypoints[i]->setId(i); + } + } + waypoints[new_seq] = t; + //waypoints[new_seq]->setId(new_seq); + + emit waypointListChanged(); + } +} + +void UASWaypointManager::localSaveWaypoints(const QString &saveFile) +{ + QFile file(saveFile); + if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) + return; + + QTextStream out(&file); + for (int i = 0; i < waypoints.size(); i++) + { + waypoints[i]->save(out); + } + file.close(); +} + +void UASWaypointManager::localLoadWaypoints(const QString &loadFile) +{ + QFile file(loadFile); + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) + return; + + while(waypoints.size()>0) + { + Waypoint *t = waypoints[0]; + waypoints.remove(0); + delete t; + } + + QTextStream in(&file); + while (!in.atEnd()) + { + Waypoint *t = new Waypoint(); + if(t->load(in)) + { + t->setId(waypoints.size()); + waypoints.insert(waypoints.size(), t); + } + } + file.close(); + + emit waypointListChanged(); +} + +void UASWaypointManager::clearWaypointList() { if(current_state == WP_IDLE) { protocol_timer.start(PROTOCOL_TIMEOUT_MS); current_retries = PROTOCOL_MAX_RETRIES; - current_state = WP_SETCURRENT; - current_wp_id = seq; + current_state = WP_CLEARLIST; + current_wp_id = 0; current_partner_systemid = uas.getUASID(); current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER; - sendWaypointSetCurrent(current_wp_id); + sendWaypointClearAll(); } } @@ -266,6 +406,13 @@ void UASWaypointManager::readWaypoints() { if(current_state == WP_IDLE) { + while(waypoints.size()>0) + { + Waypoint *t = waypoints.back(); + delete t; + waypoints.pop_back(); + } + protocol_timer.start(PROTOCOL_TIMEOUT_MS); current_retries = PROTOCOL_MAX_RETRIES; @@ -300,6 +447,8 @@ void UASWaypointManager::writeWaypoints() waypoint_buffer.pop_back(); } + bool noCurrent = true; + //copy waypoint data to local buffer for (int i=0; i < current_count; i++) { @@ -309,17 +458,20 @@ void UASWaypointManager::writeWaypoints() const Waypoint *cur_s = waypoints.at(i); cur_d->autocontinue = cur_s->getAutoContinue(); - cur_d->current = cur_s->getCurrent(); + cur_d->current = cur_s->getCurrent() & noCurrent; //make sure only one current waypoint is selected, the first selected will be chosen cur_d->orbit = 0; cur_d->orbit_direction = 0; cur_d->param1 = cur_s->getOrbit(); cur_d->param2 = cur_s->getHoldTime(); - cur_d->type = 1; //FIXME - cur_d->seq = i; + cur_d->type = 1; //FIXME: we only use local waypoints at the moment + cur_d->seq = i; // don't read out the sequence number of the waypoint class cur_d->x = cur_s->getX(); cur_d->y = cur_s->getY(); cur_d->z = cur_s->getZ(); cur_d->yaw = cur_s->getYaw(); + + if (cur_s->getCurrent() && noCurrent) + noCurrent = false; } //send the waypoint count to UAS (this starts the send transaction) @@ -341,8 +493,7 @@ void UASWaypointManager::sendWaypointClearAll() wpca.target_system = uas.getUASID(); wpca.target_component = MAV_COMP_ID_WAYPOINTPLANNER; - const QString str = QString("clearing waypoint list..."); - emit updateStatusString(str); + emit updateStatusString(QString("clearing waypoint list...")); mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca); uas.sendMessage(message); @@ -360,8 +511,7 @@ void UASWaypointManager::sendWaypointSetCurrent(quint16 seq) wpsc.target_component = MAV_COMP_ID_WAYPOINTPLANNER; wpsc.seq = seq; - const QString str = QString("Updating target waypoint..."); - emit updateStatusString(str); + emit updateStatusString(QString("Updating target waypoint...")); mavlink_msg_waypoint_set_current_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpsc); uas.sendMessage(message); @@ -379,8 +529,7 @@ void UASWaypointManager::sendWaypointCount() wpc.target_component = MAV_COMP_ID_WAYPOINTPLANNER; wpc.count = current_count; - const QString str = QString("start transmitting waypoints..."); - emit updateStatusString(str); + emit updateStatusString(QString("start transmitting waypoints...")); mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc); uas.sendMessage(message); @@ -397,8 +546,7 @@ void UASWaypointManager::sendWaypointRequestList() wprl.target_system = uas.getUASID(); wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER; - const QString str = QString("requesting waypoint list..."); - emit updateStatusString(str); + emit updateStatusString(QString("requesting waypoint list...")); mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl); uas.sendMessage(message); @@ -416,8 +564,7 @@ void UASWaypointManager::sendWaypointRequest(quint16 seq) wpr.target_component = MAV_COMP_ID_WAYPOINTPLANNER; wpr.seq = seq; - const QString str = QString("retrieving waypoint ID %1 of %2 total").arg(wpr.seq).arg(current_count); - emit updateStatusString(str); + emit updateStatusString(QString("retrieving waypoint ID %1 of %2 total").arg(wpr.seq).arg(current_count)); mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr); uas.sendMessage(message); @@ -438,8 +585,7 @@ void UASWaypointManager::sendWaypoint(quint16 seq) wp->target_system = uas.getUASID(); wp->target_component = MAV_COMP_ID_WAYPOINTPLANNER; - const QString str = QString("sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count); - emit updateStatusString(str); + emit updateStatusString(QString("sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count)); mavlink_msg_waypoint_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, wp); uas.sendMessage(message); diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index fa78e21c3..e21d4cd6b 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -75,7 +75,23 @@ public: void handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_current_t *wpc); ///< Handles received set current waypoint messages /*@}*/ - QVector &getWaypointList(void) { return waypoints; } ///< Returns a reference to the local waypoint list. Gives full access to the internal data structure - Subject to change: Public const access and friend access for the waypoint list widget. + /** @name Remote operations */ + /*@{*/ + void clearWaypointList(); ///< Sends the waypoint clear all message to the MAV + void readWaypoints(); ///< Requests the MAV's current waypoint list + void writeWaypoints(); ///< Sends the local waypoint list to the MAV + int setCurrentWaypoint(quint16 seq); ///< Changes the current waypoint and sends the sequence number of the waypoint that should get the new target waypoint to the UAS + /*@}*/ + + /** @name Local waypoint list operations */ + /*@{*/ + const QVector &getWaypointList(void) { return waypoints; } ///< Returns a const reference to the local waypoint list. + void localAddWaypoint(Waypoint *wp); ///< locally adds a new waypoint to the end of the list and changes its sequence number accordingly + int localRemoveWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage + void localMoveWaypoint(quint16 cur_seq, quint16 new_seq); ///< locally move a waypoint from its current position cur_seq to a new position new_seq + void localSaveWaypoints(const QString &saveFile); ///< saves the local waypoint list to saveFile + void localLoadWaypoints(const QString &loadFile); ///< loads a waypoint list from loadFile + /*@}*/ private: /** @name Message send functions */ @@ -91,15 +107,11 @@ private: public slots: void timeout(); ///< Called by the timer if a response times out. Handles send retries. - void setCurrent(quint16 seq); ///< Sends the sequence number of the waypoint that should get the new target waypoint - void clearWaypointList(); ///< Sends the waypoint clear all message to the MAV - void readWaypoints(); ///< Requests the MAV's current waypoint list - void writeWaypoints(); ///< Sends the local waypoint list to the MAV signals: - void waypointUpdated(quint16,double,double,double,double,bool,bool,double,int); ///< Adds a waypoint to the waypoint list widget - void currentWaypointChanged(quint16); ///< emits the new current waypoint sequence number - void updateStatusString(const QString &); ///< emits the current status string + void waypointListChanged(void); ///< emits signal that the local waypoint list has been changed + void currentWaypointChanged(quint16); ///< emits the new current waypoint sequence number + void updateStatusString(const QString &); ///< emits the current status string private: UAS &uas; ///< Reference to the corresponding UAS diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index b025febf3..7ec88638f 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -632,7 +632,7 @@ void HSIDisplay::drawWaypoints(QPainter& painter) { if (uas) { - QVector& list = uas->getWaypointManager().getWaypointList(); + const QVector& list = uas->getWaypointManager().getWaypointList(); QColor color; painter.setBrush(Qt::NoBrush); diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 6c3828f09..f9fdace47 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -55,8 +55,6 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : listLayout->setAlignment(Qt::AlignTop); m_ui->listWidget->setLayout(listLayout); - wpViews = QMap(); - this->uas = NULL; // ADD WAYPOINT @@ -120,41 +118,36 @@ void WaypointList::setUAS(UASInterface* uas) { this->uas = uas; - connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &))); - connect(&uas->getWaypointManager(), SIGNAL(waypointUpdated(quint16,double,double,double,double,bool,bool,double,int)), this, SLOT(setWaypoint(quint16,double,double,double,double,bool,bool,double,int))); - connect(&uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); - connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); - connect(this, SIGNAL(sendWaypoints()), &uas->getWaypointManager(), SLOT(writeWaypoints())); - connect(this, SIGNAL(requestWaypoints()), &uas->getWaypointManager(), SLOT(readWaypoints())); - connect(this, SIGNAL(clearWaypointList()), &uas->getWaypointManager(), SLOT(clearWaypointList())); - connect(this, SIGNAL(setCurrent(quint16)), &uas->getWaypointManager(), SLOT(setCurrent(quint16))); + 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(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); } } -void WaypointList::setWaypoint(quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime) +void WaypointList::waypointReached(quint16 waypointId) { if (this->uas) { - Waypoint* wp = new Waypoint(id, x, y, z, yaw, autocontinue, current, orbit, holdTime); - addWaypoint(wp); + updateStatusLabel(QString("Waypoint %1 reached.").arg(waypointId)); } } -void WaypointList::waypointReached(quint16 waypointId) +void WaypointList::changeCurrentWaypoint(quint16 seq) { if (this->uas) { - updateStatusLabel(QString("Waypoint %1 reached.").arg(waypointId)); + uas->getWaypointManager().setCurrentWaypoint(seq); } } -void WaypointList::changeCurrentWaypoint(quint16 seq) +void WaypointList::currentWaypointChanged(quint16 seq) { if (this->uas) { - QVector &waypoints = uas->getWaypointManager().getWaypointList(); + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); if (seq < waypoints.size()) { @@ -164,45 +157,67 @@ void WaypointList::changeCurrentWaypoint(quint16 seq) if (waypoints[i]->getId() == seq) { - waypoints[i]->setCurrent(true); widget->setCurrent(true); - emit setCurrent(seq); } else { - waypoints[i]->setCurrent(false); widget->setCurrent(false); } } - redrawList(); } } } -void WaypointList::currentWaypointChanged(quint16 seq) +void WaypointList::waypointListChanged() { if (this->uas) { - QVector &waypoints = uas->getWaypointManager().getWaypointList(); + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); - if (seq < waypoints.size()) + // first remove all views of non existing waypoints + if (!wpViews.empty()) { - for(int i = 0; i < waypoints.size(); i++) + QMapIterator viewIt(wpViews); + viewIt.toFront(); + while(viewIt.hasNext()) { - WaypointView* widget = wpViews.find(waypoints[i]).value(); - - if (waypoints[i]->getId() == seq) + viewIt.next(); + Waypoint *cur = viewIt.key(); + int i; + for (i = 0; i < waypoints.size(); i++) { - waypoints[i]->setCurrent(true); - widget->setCurrent(true); + if (waypoints[i] == cur) + { + break; + } } - else + if (i == waypoints.size()) { - waypoints[i]->setCurrent(false); - widget->setCurrent(false); + WaypointView* widget = wpViews.find(cur).value(); + widget->hide(); + listLayout->removeWidget(widget); + wpViews.remove(cur); } } - redrawList(); + } + + for(int i = 0; i < waypoints.size(); i++) + { + Waypoint *wp = waypoints[i]; + if (!wpViews.contains(wp)) + { + WaypointView* wpview = new WaypointView(wp, this); + wpViews.insert(wp, wpview); + connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); + connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); + connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); + connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); + connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); + } + + WaypointView *wpv = wpViews.value(wp); + wpv->updateValues(); // update the values of the ui elements in the view + listLayout->addWidget(wpv); } } } @@ -211,21 +226,16 @@ void WaypointList::read() { if (uas) { - QVector &waypoints = uas->getWaypointManager().getWaypointList(); - - while(waypoints.size()>0) - { - removeWaypoint(waypoints[0]); - } - - emit requestWaypoints(); + uas->getWaypointManager().readWaypoints(); } } void WaypointList::transmit() { - emit sendWaypoints(); - //emit requestWaypoints(); FIXME + if (uas) + { + uas->getWaypointManager().writeWaypoints(); + } } void WaypointList::add() @@ -233,16 +243,17 @@ void WaypointList::add() // Only add waypoints if UAS is present if (uas) { - QVector &waypoints = uas->getWaypointManager().getWaypointList(); - + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); if (waypoints.size() > 0) { Waypoint *last = waypoints.at(waypoints.size()-1); - addWaypoint(new Waypoint(waypoints.size(), last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime())); + Waypoint *wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()); + uas->getWaypointManager().localAddWaypoint(wp); } else { - addWaypoint(new Waypoint(waypoints.size(), 1.1, 1.1, -0.8, 0.0, true, true, 0.15, 2000)); + Waypoint *wp = new Waypoint(0, 1.1, 1.1, -0.8, 0.0, true, true, 0.15, 2000); + uas->getWaypointManager().localAddWaypoint(wp); } } } @@ -251,119 +262,61 @@ void WaypointList::addCurrentPositonWaypoint() { if (uas) { - QVector &waypoints = uas->getWaypointManager().getWaypointList(); - + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); if (waypoints.size() > 0) { Waypoint *last = waypoints.at(waypoints.size()-1); - addWaypoint(new Waypoint(waypoints.size(), (float)(qRound(mavX*100))/100.f, (float)(qRound(mavY*100))/100.f, (float)(qRound(mavZ*100))/100.f, (float)(qRound(mavYaw*100))/100.f, last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime())); + Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()); + uas->getWaypointManager().localAddWaypoint(wp); } else { - addWaypoint(new Waypoint(waypoints.size(), (float)(qRound(mavX*100))/100.f, (float)(qRound(mavY*100))/100.f, (float)(qRound(mavZ*100))/100.f, (float)(qRound(mavYaw*100))/100.f, true, true, 0.15, 2000)); + Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., true, true, 0.15, 2000); + uas->getWaypointManager().localAddWaypoint(wp); } - } } -void WaypointList::addWaypoint(Waypoint* wp) +void WaypointList::moveUp(Waypoint* wp) { if (uas) { - uas->getWaypointManager().getWaypointList().push_back(wp); - if (!wpViews.contains(wp)) + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); + + //get the current position of wp in the local storage + int i; + for (i = 0; i < waypoints.size(); i++) { - WaypointView* wpview = new WaypointView(wp, this); - wpViews.insert(wp, wpview); - listLayout->addWidget(wpViews.value(wp)); - 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))); + if (waypoints[i] == wp) + break; } - } -} - -void WaypointList::redrawList() -{ - if (uas) - { - QVector &waypoints = uas->getWaypointManager().getWaypointList(); - // Clear list layout - if (!wpViews.empty()) + // if wp was found and its not the first entry, move it + if (i < waypoints.size() && i > 0) { - QMapIterator viewIt(wpViews); - viewIt.toFront(); - while(viewIt.hasNext()) - { - viewIt.next(); - listLayout->removeWidget(viewIt.value()); - } - // Re-add waypoints - for(int i = 0; i < waypoints.size(); i++) - { - listLayout->addWidget(wpViews.value(waypoints[i])); - } + uas->getWaypointManager().localMoveWaypoint(i, i-1); } } } -void WaypointList::moveUp(Waypoint* wp) +void WaypointList::moveDown(Waypoint* wp) { if (uas) { - QVector &waypoints = uas->getWaypointManager().getWaypointList(); + const QVector &waypoints = uas->getWaypointManager().getWaypointList(); - int id = wp->getId(); - if (waypoints.size() > 1 && waypoints.size() > id) + //get the current position of wp in the local storage + int i; + for (i = 0; i < waypoints.size(); i++) { - Waypoint* temp = waypoints[id]; - if (id > 0) - { - waypoints[id] = waypoints[id-1]; - waypoints[id-1] = temp; - waypoints[id-1]->setId(id-1); - waypoints[id]->setId(id); - } - else - { - waypoints[id] = waypoints[waypoints.size()-1]; - waypoints[waypoints.size()-1] = temp; - waypoints[waypoints.size()-1]->setId(waypoints.size()-1); - waypoints[id]->setId(id); - } - redrawList(); + if (waypoints[i] == wp) + break; } - } -} -void WaypointList::moveDown(Waypoint* wp) -{ - if (uas) - { - QVector &waypoints = uas->getWaypointManager().getWaypointList(); - - int id = wp->getId(); - if (waypoints.size() > 1 && waypoints.size() > id) + // if wp was found and its not the last entry, move it + if (i < waypoints.size()-1) { - Waypoint* temp = waypoints[id]; - if (id != waypoints.size()-1) - { - waypoints[id] = waypoints[id+1]; - waypoints[id+1] = temp; - waypoints[id+1]->setId(id+1); - waypoints[id]->setId(id); - } - else - { - waypoints[id] = waypoints[0]; - waypoints[0] = temp; - waypoints[0]->setId(0); - waypoints[id]->setId(id); - } - redrawList(); + uas->getWaypointManager().localMoveWaypoint(i, i+1); } } } @@ -372,24 +325,7 @@ void WaypointList::removeWaypoint(Waypoint* wp) { if (uas) { - QVector &waypoints = uas->getWaypointManager().getWaypointList(); - - // Delete from list - if (wp != NULL) - { - waypoints.remove(wp->getId()); - for(int i = wp->getId(); i < waypoints.size(); i++) - { - waypoints[i]->setId(i); - } - - // Remove from view - WaypointView* widget = wpViews.find(wp).value(); - wpViews.remove(wp); - widget->hide(); - listLayout->removeWidget(widget); - delete wp; - } + uas->getWaypointManager().localRemoveWaypoint(wp->getId()); } } @@ -409,20 +345,7 @@ void WaypointList::saveWaypoints() if (uas) { QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)")); - QFile file(fileName); - if (!file.open(QIODevice::WriteOnly | QIODevice::Text)) - return; - - QVector &waypoints = uas->getWaypointManager().getWaypointList(); - - QTextStream in(&file); - for (int i = 0; i < waypoints.size(); i++) - { - Waypoint* wp = waypoints[i]; - in << "\t" << wp->getId() << "\t" << wp->getX() << "\t" << wp->getY() << "\t" << wp->getZ() << "\t" << wp->getYaw() << "\t" << wp->getAutoContinue() << "\t" << wp->getCurrent() << "\t" << wp->getOrbit() << "\t" << wp->getHoldTime() << "\n"; - in.flush(); - } - file.close(); + uas->getWaypointManager().localSaveWaypoints(fileName); } } @@ -431,25 +354,7 @@ void WaypointList::loadWaypoints() if (uas) { QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)")); - QFile file(fileName); - if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) - return; - - QVector &waypoints = uas->getWaypointManager().getWaypointList(); - - while(waypoints.size()>0) - { - removeWaypoint(waypoints[0]); - } - - QTextStream in(&file); - while (!in.atEnd()) - { - QStringList wpParams = in.readLine().split("\t"); - if (wpParams.size() == 10) - addWaypoint(new Waypoint(wpParams[1].toInt(), wpParams[2].toDouble(), wpParams[3].toDouble(), wpParams[4].toDouble(), wpParams[5].toDouble(), (wpParams[6].toInt() == 1 ? true : false), (wpParams[7].toInt() == 1 ? true : false), wpParams[8].toDouble(), wpParams[9].toInt())); - } - file.close(); + uas->getWaypointManager().localLoadWaypoints(fileName); } } diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index 4b1236fd8..0d3ff2745 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -55,7 +55,6 @@ class WaypointList : public QWidget { public slots: void setUAS(UASInterface* uas); - void redrawList(); //UI Buttons void saveWaypoints(); @@ -73,23 +72,18 @@ public slots: void updateStatusLabel(const QString &string); void changeCurrentWaypoint(quint16 seq); ///< The user wants to change the current waypoint - void currentWaypointChanged(quint16 seq); ///< The waypointplanner changed the current waypoint - void setWaypoint(quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime); - void addWaypoint(Waypoint* wp); + void currentWaypointChanged(quint16 seq); ///< The waypoint planner changed the current waypoint + void waypointListChanged(void); ///< The waypoint manager informs that the waypoint list was changed + void removeWaypoint(Waypoint* wp); void waypointReached(quint16 waypointId); void updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec); void updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64 usec); -signals: - void sendWaypoints(); - void requestWaypoints(); - void clearWaypointList(); - void setCurrent(quint16); - protected: virtual void changeEvent(QEvent *e); +protected: QMap wpViews; QVBoxLayout* listLayout; UASInterface* uas; diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc index a9ec214de..13c772e23 100644 --- a/src/ui/WaypointView.cc +++ b/src/ui/WaypointView.cc @@ -48,15 +48,7 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : this->wp = wp; // Read values and set user interface - m_ui->xSpinBox->setValue(wp->getX()); - m_ui->ySpinBox->setValue(wp->getY()); - m_ui->zSpinBox->setValue(wp->getZ()); - m_ui->yawSpinBox->setValue(wp->getYaw()/M_PI*180.); - m_ui->selectedBox->setChecked(wp->getCurrent()); - m_ui->autoContinue->setChecked(wp->getAutoContinue()); - m_ui->idLabel->setText(QString("%1").arg(wp->getId()));\ - m_ui->orbitSpinBox->setValue(wp->getOrbit()); - m_ui->holdTimeSpinBox->setValue(wp->getHoldTime()); + updateValues(); connect(m_ui->xSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double))); connect(m_ui->ySpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double))); @@ -110,15 +102,30 @@ void WaypointView::changedCurrent(int state) { if (state == 0) { - wp->setCurrent(false); + //m_ui->selectedBox->setChecked(true); + //m_ui->selectedBox->setCheckState(Qt::Checked); + //wp->setCurrent(false); } else { - wp->setCurrent(true); + //wp->setCurrent(true); emit changeCurrentWaypoint(wp->getId()); //the slot changeCurrentWaypoint() in WaypointList sets all other current flags to false } } +void WaypointView::updateValues() +{ + m_ui->xSpinBox->setValue(wp->getX()); + m_ui->ySpinBox->setValue(wp->getY()); + m_ui->zSpinBox->setValue(wp->getZ()); + m_ui->yawSpinBox->setValue(wp->getYaw()/M_PI*180.); + m_ui->selectedBox->setChecked(wp->getCurrent()); + m_ui->autoContinue->setChecked(wp->getAutoContinue()); + m_ui->idLabel->setText(QString("%1").arg(wp->getId()));\ + m_ui->orbitSpinBox->setValue(wp->getOrbit()); + m_ui->holdTimeSpinBox->setValue(wp->getHoldTime()); +} + void WaypointView::setCurrent(bool state) { if (state) diff --git a/src/ui/WaypointView.h b/src/ui/WaypointView.h index bee5e1855..1db39bc88 100644 --- a/src/ui/WaypointView.h +++ b/src/ui/WaypointView.h @@ -57,6 +57,7 @@ public slots: void remove(); void changedAutoContinue(int); void changedCurrent(int); + void updateValues(void); void setYaw(int); //hidden degree to radian conversion -- 2.22.0