Skip to content
GitLab
Commit 19f73a78 authored by pixhawk's avatar pixhawk
Browse files

moved the local waypoint list into waypoint manager

parent 2438b8c8
Hide whitespace changes
Inline Side-by-side
src/uas/UASWaypointManager.cc
View file @ 19f73a78
......@@ -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<Waypoint*> &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<Waypoint*> &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();
......
src/uas/UASWaypointManager.h
View file @ 19f73a78
......@@ -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<Waypoint *> &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<Waypoint *> &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<Waypoint *> waypoints; ///< local waypoint list
QVector<mavlink_waypoint_t *> waypoint_buffer; ///< communication buffer for waypoints
QTimer protocol_timer; ///< Timer to catch timeouts
};
......
src/ui/WaypointList.cc
View file @ 19f73a78
......@@ -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<Waypoint*> &)), &uas->getWaypointManager(), SLOT(sendWaypoints(const QVector<Waypoint*> &)));
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<Waypoint *> &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<Waypoint *> &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<Waypoint *> &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<Waypoint *> &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<Waypoint*,WaypointView*> 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<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
// Clear list layout
if (!wpViews.empty())
{
listLayout->addWidget(wpViews.value(waypoints[i]));
QMapIterator<Waypoint*,WaypointView*> 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<Waypoint *> &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<Waypoint *> &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<Waypoint *> &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<Waypoint *> &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<Waypoint *> &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();
}
src/ui/WaypointList.h
View file @ 19f73a78
......@@ -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<Waypoint*> &);
void sendWaypoints();
void requestWaypoints();
void clearWaypointList();
protected:
virtual void changeEvent(QEvent *e);
QVector<Waypoint*> waypoints;
QMap<Waypoint*, WaypointView*> wpViews;
QVBoxLayout* listLayout;
UASInterface* uas;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment