diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc index 05523810cfc7c5cc9dbe2db5613e7ebe6dff284b..9a54fcf786e5d0bbddd85b1d33df39c7ae8487b9 100644 --- a/src/uas/UASWaypointManager.cc +++ b/src/uas/UASWaypointManager.cc @@ -94,7 +94,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ if(wp->seq == current_wp_id) { //update the UI FIXME - emit waypointUpdated(uas.getUASID(), wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->orbit, wp->hold_time); + emit waypointUpdated(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->orbit, wp->hold_time); //get next waypoint current_wp_id++; @@ -206,15 +206,15 @@ void UASWaypointManager::requestWaypoints() } } -void UASWaypointManager::sendWaypoints(const QVector &list) +void UASWaypointManager::sendWaypoints() { if (current_state == WP_IDLE) { - if (list.count() > 0) + if (waypoints.count() > 0) { protocol_timer.start(PROTOCOL_TIMEOUT_MS); - current_count = list.count(); + current_count = waypoints.count(); current_state = WP_SENDLIST; current_wp_id = 0; current_partner_systemid = uas.getUASID(); @@ -233,7 +233,7 @@ void UASWaypointManager::sendWaypoints(const QVector &list) waypoint_buffer.push_back(new mavlink_waypoint_t); mavlink_waypoint_t *cur_d = waypoint_buffer.back(); memset(cur_d, 0, sizeof(mavlink_waypoint_t)); //initialize with zeros - const Waypoint *cur_s = list.at(i); + const Waypoint *cur_s = waypoints.at(i); cur_d->autocontinue = cur_s->getAutoContinue(); cur_d->current = cur_s->getCurrent(); diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h index 8e7e7458453b96c9cee4c9537b3813a9448af043..f8432d31bb35a1bc99d8dfe5b19d9faac17744c7 100644 --- a/src/uas/UASWaypointManager.h +++ b/src/uas/UASWaypointManager.h @@ -60,6 +60,8 @@ public: void handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr); void handleWaypointSetCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_set_current_t *wpr); + QVector &getWaypointList(void) { return waypoints; } + private: void sendWaypointRequest(quint16 seq); void sendWaypoint(quint16 seq); @@ -68,12 +70,12 @@ public slots: void timeout(); void clearWaypointList(); void requestWaypoints(); - void sendWaypoints(const QVector &list); + void sendWaypoints(); signals: - void waypointUpdated(int,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 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 private: UAS &uas; ///< Reference to the corresponding UAS @@ -83,6 +85,7 @@ private: quint8 current_partner_systemid; ///< The current protocol communication target system quint8 current_partner_compid; ///< The current protocol communication target component + QVector waypoints; ///< local waypoint list QVector waypoint_buffer; ///< communication buffer for waypoints QTimer protocol_timer; ///< Timer to catch timeouts }; diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index ff0acf160ac50997190cf26b36699f046776fe49..42c8cd5edb6322c1ac89bd34417f9b1edc95ad98 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -79,11 +79,11 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); - // STATUS LABEL - updateStatusLabel(""); - // SET UAS AFTER ALL SIGNALS/SLOTS ARE CONNECTED setUAS(uas); + + // STATUS LABEL + updateStatusLabel(""); } WaypointList::~WaypointList() @@ -93,7 +93,10 @@ WaypointList::~WaypointList() void WaypointList::updateStatusLabel(const QString &string) { - m_ui->statusLabel->setText(string); + if (this->uas) + { + m_ui->statusLabel->setText(string); + } } void WaypointList::updateLocalPosition(UASInterface* uas, double x, double y, double z, quint64 usec) @@ -120,71 +123,81 @@ 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(int,quint16,double,double,double,double,bool,bool,double,int)), this, SLOT(setWaypoint(int,quint16,double,double,double,double,bool,bool,double,int))); - connect(&uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); + 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(this, SIGNAL(sendWaypoints(const QVector &)), &uas->getWaypointManager(), SLOT(sendWaypoints(const QVector &))); - connect(this, SIGNAL(requestWaypoints()), &uas->getWaypointManager(), SLOT(requestWaypoints())); - connect(this, SIGNAL(clearWaypointList()), &uas->getWaypointManager(), SLOT(clearWaypointList())); + connect(this, SIGNAL(sendWaypoints()), &uas->getWaypointManager(), SLOT(sendWaypoints())); + connect(this, SIGNAL(requestWaypoints()), &uas->getWaypointManager(), SLOT(requestWaypoints())); + connect(this, SIGNAL(clearWaypointList()), &uas->getWaypointManager(), SLOT(clearWaypointList())); } } -void WaypointList::setWaypoint(int uasId, quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime) +void WaypointList::setWaypoint(quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime) { - if (uasId == this->uas->getUASID()) + if (this->uas) { Waypoint* wp = new Waypoint(id, x, y, z, yaw, autocontinue, current, orbit, holdTime); addWaypoint(wp); } } -void WaypointList::waypointReached(UASInterface* uas, quint16 waypointId) +void WaypointList::waypointReached(quint16 waypointId) { - Q_UNUSED(uas); - qDebug() << "Waypoint reached: " << waypointId; - - updateStatusLabel(QString("Waypoint %1 reached.").arg(waypointId)); + if (this->uas) + { + updateStatusLabel(QString("Waypoint %1 reached.").arg(waypointId)); + } } void WaypointList::currentWaypointChanged(quint16 seq) { - if (seq < waypoints.size()) + if (this->uas) { - for(int i = 0; i < waypoints.size(); i++) - { - WaypointView* widget = wpViews.find(waypoints[i]).value(); + QVector &waypoints = uas->getWaypointManager().getWaypointList(); - if (waypoints[i]->getId() == seq) - { - waypoints[i]->setCurrent(true); - widget->setCurrent(true); - } - else + if (seq < waypoints.size()) + { + for(int i = 0; i < waypoints.size(); i++) { - waypoints[i]->setCurrent(false); - widget->setCurrent(false); + WaypointView* widget = wpViews.find(waypoints[i]).value(); + + if (waypoints[i]->getId() == seq) + { + waypoints[i]->setCurrent(true); + widget->setCurrent(true); + } + else + { + waypoints[i]->setCurrent(false); + widget->setCurrent(false); + } } + redrawList(); } - redrawList(); } } void WaypointList::read() { - while(waypoints.size()>0) + if (uas) { - removeWaypoint(waypoints[0]); - } + QVector &waypoints = uas->getWaypointManager().getWaypointList(); - emit requestWaypoints(); + while(waypoints.size()>0) + { + removeWaypoint(waypoints[0]); + } + + emit requestWaypoints(); + } } void WaypointList::transmit() { - emit sendWaypoints(waypoints); + emit sendWaypoints(); //emit requestWaypoints(); FIXME } @@ -193,6 +206,8 @@ void WaypointList::add() // Only add waypoints if UAS is present if (uas) { + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + if (waypoints.size() > 0) { Waypoint *last = waypoints.at(waypoints.size()-1); @@ -209,6 +224,8 @@ void WaypointList::addCurrentPositonWaypoint() { if (uas) { + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + if (waypoints.size() > 0) { Waypoint *last = waypoints.at(waypoints.size()-1); @@ -224,104 +241,127 @@ void WaypointList::addCurrentPositonWaypoint() void WaypointList::addWaypoint(Waypoint* wp) { - waypoints.push_back(wp); - if (!wpViews.contains(wp)) + if (uas) { - 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))); + uas->getWaypointManager().getWaypointList().push_back(wp); + if (!wpViews.contains(wp)) + { + 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))); + } } } void WaypointList::redrawList() { - // Clear list layout - if (!wpViews.empty()) + if (uas) { - 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++) + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + + // Clear list layout + if (!wpViews.empty()) { - listLayout->addWidget(wpViews.value(waypoints[i])); + 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])); + } } } } void WaypointList::moveUp(Waypoint* wp) { - int id = wp->getId(); - if (waypoints.size() > 1 && waypoints.size() > id) + if (uas) { - 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 + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + + int id = wp->getId(); + if (waypoints.size() > 1 && waypoints.size() > id) { - waypoints[id] = waypoints[waypoints.size()-1]; - waypoints[waypoints.size()-1] = temp; - waypoints[waypoints.size()-1]->setId(waypoints.size()-1); - waypoints[id]->setId(id); + 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(); } - redrawList(); } } void WaypointList::moveDown(Waypoint* wp) { - int id = wp->getId(); - if (waypoints.size() > 1 && waypoints.size() > id) + if (uas) { - 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 + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + + int id = wp->getId(); + if (waypoints.size() > 1 && waypoints.size() > id) { - waypoints[id] = waypoints[0]; - waypoints[0] = temp; - waypoints[0]->setId(0); - waypoints[id]->setId(id); + 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(); } - redrawList(); } } void WaypointList::removeWaypoint(Waypoint* wp) { - // Delete from list - if (wp != NULL) + if (uas) { - waypoints.remove(wp->getId()); - for(int i = wp->getId(); i < waypoints.size(); i++) + QVector &waypoints = uas->getWaypointManager().getWaypointList(); + + // Delete from list + if (wp != NULL) { - waypoints[i]->setId(i); - } + 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; + // Remove from view + WaypointView* widget = wpViews.find(wp).value(); + wpViews.remove(wp); + widget->hide(); + listLayout->removeWidget(widget); + delete wp; + } } } @@ -338,40 +378,50 @@ void WaypointList::changeEvent(QEvent *e) void WaypointList::saveWaypoints() { - 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; - - QTextStream in(&file); - for (int i = 0; i < waypoints.size(); i++) + if (uas) { - 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(); + 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(); } - file.close(); } void WaypointList::loadWaypoints() { - QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)")); - QFile file(fileName); - if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) - return; - - while(waypoints.size()>0) + if (uas) { - removeWaypoint(waypoints[0]); - } + QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)")); + QFile file(fileName); + if (!file.open(QIODevice::ReadOnly | QIODevice::Text)) + return; - 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())); + 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(); } - file.close(); } diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h index 1494edfdd9f855c2d63f2b30b28495772d1f4d19..111f7a8867801a297e2bb499da36f820787f2f40 100644 --- a/src/ui/WaypointList.h +++ b/src/ui/WaypointList.h @@ -73,24 +73,21 @@ public slots: void updateStatusLabel(const QString &string); void currentWaypointChanged(quint16 seq); - - //To be moved to UASWaypointManager (?) - void setWaypoint(int uasId, quint16 id, double x, double y, double z, double yaw, bool autocontinue, bool current, double orbit, int holdTime); + 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 removeWaypoint(Waypoint* wp); - void waypointReached(UASInterface* uas, quint16 waypointId); + 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(const QVector &); + void sendWaypoints(); void requestWaypoints(); void clearWaypointList(); protected: virtual void changeEvent(QEvent *e); - QVector waypoints; QMap wpViews; QVBoxLayout* listLayout; UASInterface* uas;