Skip to content
GitLab
Commit 11fa19c3 authored by pixhawk's avatar pixhawk
Browse files

Major refactoring of the waypoint code. The new design fits the... 

Major refactoring of the waypoint code. The new design fits the model-view-controller pattern better.
parent 77f57fe8
Hide whitespace changes
Inline Side-by-side
src/Waypoint.cc
View file @ 11fa19c3
...@@ -31,6 +31,7 @@ This file is part of the PIXHAWK project ...@@ -31,6 +31,7 @@ This file is part of the PIXHAWK project
*/ */
#include "Waypoint.h" #include "Waypoint.h"
#include <QStringList>
Waypoint::Waypoint(quint16 _id, float _x, float _y, float _z, float _yaw, bool _autocontinue, bool _current, float _orbit, int _holdTime) Waypoint::Waypoint(quint16 _id, float _x, float _y, float _z, float _yaw, bool _autocontinue, bool _current, float _orbit, int _holdTime)
: id(_id), : id(_id),
...@@ -50,6 +51,30 @@ Waypoint::~Waypoint() ...@@ -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) void Waypoint::setId(quint16 id)
{ {
this->id = id; this->id = id;
......
src/Waypoint.h
View file @ 11fa19c3
...@@ -35,6 +35,7 @@ This file is part of the PIXHAWK project ...@@ -35,6 +35,7 @@ This file is part of the PIXHAWK project
#include <QObject> #include <QObject>
#include <QString> #include <QString>
#include <QTextStream>
class Waypoint : public QObject class Waypoint : public QObject
{ {
...@@ -54,6 +55,10 @@ public: ...@@ -54,6 +55,10 @@ public:
float getOrbit() const { return orbit; } float getOrbit() const { return orbit; }
float getHoldTime() const { return holdTime; } float getHoldTime() const { return holdTime; }
void save(QTextStream &saveStream);
bool load(QTextStream &loadStream);
private: private:
quint16 id; quint16 id;
float x; float x;
......
src/uas/UASWaypointManager.cc
View file @ 11fa19c3
...@@ -131,8 +131,8 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_ ...@@ -131,8 +131,8 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
if(wp->seq == current_wp_id) if(wp->seq == current_wp_id)
{ {
//update the UI FIXME Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2);
emit waypointUpdated(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2); localAddWaypoint(lwp);
//get next waypoint //get next waypoint
current_wp_id++; current_wp_id++;
...@@ -223,42 +223,182 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m ...@@ -223,42 +223,182 @@ void UASWaypointManager::handleWaypointCurrent(quint8 systemId, quint8 compId, m
{ {
protocol_timer.stop(); protocol_timer.stop();
current_state = WP_IDLE; 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 updateStatusString(QString("New current waypoint %1").arg(wpc->seq));
//emit update to UI widgets
emit currentWaypointChanged(wpc->seq);
} }
qDebug() << "new current waypoint" << 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); if(current_state == WP_IDLE)
current_retries = PROTOCOL_MAX_RETRIES; {
//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; //TODO: signal changed waypoint list
current_wp_id = 0;
current_partner_systemid = uas.getUASID();
current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
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) if(current_state == WP_IDLE)
{ {
protocol_timer.start(PROTOCOL_TIMEOUT_MS); protocol_timer.start(PROTOCOL_TIMEOUT_MS);
current_retries = PROTOCOL_MAX_RETRIES; current_retries = PROTOCOL_MAX_RETRIES;
current_state = WP_SETCURRENT; current_state = WP_CLEARLIST;
current_wp_id = seq; current_wp_id = 0;
current_partner_systemid = uas.getUASID(); current_partner_systemid = uas.getUASID();
current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER; current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
sendWaypointSetCurrent(current_wp_id); sendWaypointClearAll();
} }
} }
...@@ -266,6 +406,13 @@ void UASWaypointManager::readWaypoints() ...@@ -266,6 +406,13 @@ void UASWaypointManager::readWaypoints()
{ {
if(current_state == WP_IDLE) if(current_state == WP_IDLE)
{ {
while(waypoints.size()>0)
{
Waypoint *t = waypoints.back();
delete t;
waypoints.pop_back();
}
protocol_timer.start(PROTOCOL_TIMEOUT_MS); protocol_timer.start(PROTOCOL_TIMEOUT_MS);
current_retries = PROTOCOL_MAX_RETRIES; current_retries = PROTOCOL_MAX_RETRIES;
...@@ -300,6 +447,8 @@ void UASWaypointManager::writeWaypoints() ...@@ -300,6 +447,8 @@ void UASWaypointManager::writeWaypoints()
waypoint_buffer.pop_back(); waypoint_buffer.pop_back();
} }
bool noCurrent = true;
//copy waypoint data to local buffer //copy waypoint data to local buffer
for (int i=0; i < current_count; i++) for (int i=0; i < current_count; i++)
{ {
...@@ -309,17 +458,20 @@ void UASWaypointManager::writeWaypoints() ...@@ -309,17 +458,20 @@ void UASWaypointManager::writeWaypoints()
const Waypoint *cur_s = waypoints.at(i); const Waypoint *cur_s = waypoints.at(i);
cur_d->autocontinue = cur_s->getAutoContinue(); 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 = 0;
cur_d->orbit_direction = 0; cur_d->orbit_direction = 0;
cur_d->param1 = cur_s->getOrbit(); cur_d->param1 = cur_s->getOrbit();
cur_d->param2 = cur_s->getHoldTime(); cur_d->param2 = cur_s->getHoldTime();
cur_d->type = 1; //FIXME cur_d->type = 1; //FIXME: we only use local waypoints at the moment
cur_d->seq = i; cur_d->seq = i; // don't read out the sequence number of the waypoint class
cur_d->x = cur_s->getX(); cur_d->x = cur_s->getX();
cur_d->y = cur_s->getY(); cur_d->y = cur_s->getY();
cur_d->z = cur_s->getZ(); cur_d->z = cur_s->getZ();
cur_d->yaw = cur_s->getYaw(); cur_d->yaw = cur_s->getYaw();
if (cur_s->getCurrent() && noCurrent)
noCurrent = false;
} }
//send the waypoint count to UAS (this starts the send transaction) //send the waypoint count to UAS (this starts the send transaction)
...@@ -341,8 +493,7 @@ void UASWaypointManager::sendWaypointClearAll() ...@@ -341,8 +493,7 @@ void UASWaypointManager::sendWaypointClearAll()
wpca.target_system = uas.getUASID(); wpca.target_system = uas.getUASID();
wpca.target_component = MAV_COMP_ID_WAYPOINTPLANNER; wpca.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
const QString str = QString("clearing waypoint list..."); emit updateStatusString(QString("clearing waypoint list..."));
emit updateStatusString(str);
mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca); mavlink_msg_waypoint_clear_all_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpca);
uas.sendMessage(message); uas.sendMessage(message);
...@@ -360,8 +511,7 @@ void UASWaypointManager::sendWaypointSetCurrent(quint16 seq) ...@@ -360,8 +511,7 @@ void UASWaypointManager::sendWaypointSetCurrent(quint16 seq)
wpsc.target_component = MAV_COMP_ID_WAYPOINTPLANNER; wpsc.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
wpsc.seq = seq; wpsc.seq = seq;
const QString str = QString("Updating target waypoint..."); emit updateStatusString(QString("Updating target waypoint..."));
emit updateStatusString(str);
mavlink_msg_waypoint_set_current_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpsc); mavlink_msg_waypoint_set_current_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpsc);
uas.sendMessage(message); uas.sendMessage(message);
...@@ -379,8 +529,7 @@ void UASWaypointManager::sendWaypointCount() ...@@ -379,8 +529,7 @@ void UASWaypointManager::sendWaypointCount()
wpc.target_component = MAV_COMP_ID_WAYPOINTPLANNER; wpc.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
wpc.count = current_count; wpc.count = current_count;
const QString str = QString("start transmitting waypoints..."); emit updateStatusString(QString("start transmitting waypoints..."));
emit updateStatusString(str);
mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc); mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc);
uas.sendMessage(message); uas.sendMessage(message);
...@@ -397,8 +546,7 @@ void UASWaypointManager::sendWaypointRequestList() ...@@ -397,8 +546,7 @@ void UASWaypointManager::sendWaypointRequestList()
wprl.target_system = uas.getUASID(); wprl.target_system = uas.getUASID();
wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER; wprl.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
const QString str = QString("requesting waypoint list..."); emit updateStatusString(QString("requesting waypoint list..."));
emit updateStatusString(str);
mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl); mavlink_msg_waypoint_request_list_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wprl);
uas.sendMessage(message); uas.sendMessage(message);
...@@ -416,8 +564,7 @@ void UASWaypointManager::sendWaypointRequest(quint16 seq) ...@@ -416,8 +564,7 @@ void UASWaypointManager::sendWaypointRequest(quint16 seq)
wpr.target_component = MAV_COMP_ID_WAYPOINTPLANNER; wpr.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
wpr.seq = seq; wpr.seq = seq;
const QString str = QString("retrieving waypoint ID %1 of %2 total").arg(wpr.seq).arg(current_count); emit updateStatusString(QString("retrieving waypoint ID %1 of %2 total").arg(wpr.seq).arg(current_count));
emit updateStatusString(str);
mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr); mavlink_msg_waypoint_request_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpr);
uas.sendMessage(message); uas.sendMessage(message);
...@@ -438,8 +585,7 @@ void UASWaypointManager::sendWaypoint(quint16 seq) ...@@ -438,8 +585,7 @@ void UASWaypointManager::sendWaypoint(quint16 seq)
wp->target_system = uas.getUASID(); wp->target_system = uas.getUASID();
wp->target_component = MAV_COMP_ID_WAYPOINTPLANNER; 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(QString("sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count));
emit updateStatusString(str);
mavlink_msg_waypoint_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, wp); mavlink_msg_waypoint_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, wp);
uas.sendMessage(message); uas.sendMessage(message);
......
src/uas/UASWaypointManager.h
View file @ 11fa19c3
...@@ -75,7 +75,23 @@ public: ...@@ -75,7 +75,23 @@ public:
void handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_current_t *wpc); ///< Handles received set current waypoint messages void handleWaypointCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_current_t *wpc); ///< Handles received set current waypoint messages
/*@}*/ /*@}*/
QVector<Waypoint *> &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<Waypoint *> &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: private:
/** @name Message send functions */ /** @name Message send functions */
...@@ -91,15 +107,11 @@ private: ...@@ -91,15 +107,11 @@ private:
public slots: public slots:
void timeout(); ///< Called by the timer if a response times out. Handles send retries. 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: signals:
void waypointUpdated(quint16,double,double,double,double,bool,bool,double,int); ///< Adds a waypoint to the waypoint list widget void waypointListChanged(void); ///< emits signal that the local waypoint list has been changed
void currentWaypointChanged(quint16); ///< emits the new current waypoint sequence number void currentWaypointChanged(quint16); ///< emits the new current waypoint sequence number
void updateStatusString(const QString &); ///< emits the current status string void updateStatusString(const QString &); ///< emits the current status string
private: private:
UAS &uas; ///< Reference to the corresponding UAS UAS &uas; ///< Reference to the corresponding UAS
......
src/ui/HSIDisplay.cc
View file @ 11fa19c3
...@@ -632,7 +632,7 @@ void HSIDisplay::drawWaypoints(QPainter& painter) ...@@ -632,7 +632,7 @@ void HSIDisplay::drawWaypoints(QPainter& painter)
{ {
if (uas) if (uas)
{ {
QVector<Waypoint*>& list = uas->getWaypointManager().getWaypointList(); const QVector<Waypoint*>& list = uas->getWaypointManager().getWaypointList();
QColor color; QColor color;
painter.setBrush(Qt::NoBrush); painter.setBrush(Qt::NoBrush);
......
src/ui/WaypointList.cc
View file @ 11fa19c3
...@@ -55,8 +55,6 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : ...@@ -55,8 +55,6 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
listLayout->setAlignment(Qt::AlignTop); listLayout->setAlignment(Qt::AlignTop);
m_ui->listWidget->setLayout(listLayout); m_ui->listWidget->setLayout(listLayout);
wpViews = QMap<Waypoint*, WaypointView*>();
this->uas = NULL; this->uas = NULL;
// ADD WAYPOINT // ADD WAYPOINT
...@@ -120,41 +118,36 @@ void WaypointList::setUAS(UASInterface* uas) ...@@ -120,41 +118,36 @@ void WaypointList::setUAS(UASInterface* uas)
{ {
this->uas = uas; this->uas = uas;
connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &))); connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64)));
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, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64)));
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()), &uas->getWaypointManager(), SLOT(writeWaypoints())); connect(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &)));
connect(this, SIGNAL(requestWaypoints()), &uas->getWaypointManager(), SLOT(readWaypoints())); connect(&uas->getWaypointManager(), SIGNAL(waypointListChanged(void)), this, SLOT(waypointListChanged(void)));
connect(this, SIGNAL(clearWaypointList()), &uas->getWaypointManager(), SLOT(clearWaypointList())); connect(&uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
connect(this, SIGNAL(setCurrent(quint16)), &uas->getWaypointManager(), SLOT(setCurrent(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) if (this->uas)
{ {
Waypoint* wp = new Waypoint(id, x, y, z, yaw, autocontinue, current, orbit, holdTime); updateStatusLabel(QString("Waypoint %1 reached.").arg(waypointId));
addWaypoint(wp);
} }
} }
void WaypointList::waypointReached(quint16 waypointId) void WaypointList::changeCurrentWaypoint(quint16 seq)
{ {
if (this->uas) 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) if (this->uas)
{ {
QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList(); const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
if (seq < waypoints.size()) if (seq < waypoints.size())
{ {
...@@ -164,45 +157,67 @@ void WaypointList::changeCurrentWaypoint(quint16 seq) ...@@ -164,45 +157,67 @@ void WaypointList::changeCurrentWaypoint(quint16 seq)
if (waypoints[i]->getId() == seq) if (waypoints[i]->getId() == seq)
{ {
waypoints[i]->setCurrent(true);
widget->setCurrent(true); widget->setCurrent(true);
emit setCurrent(seq);
} }
else else
{ {
waypoints[i]->setCurrent(false);
widget->setCurrent(false); widget->setCurrent(false);
} }
} }
redrawList();
} }
} }
} }
void WaypointList::currentWaypointChanged(quint16 seq) void WaypointList::waypointListChanged()
{ {
if (this->uas) if (this->uas)
{ {
QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList(); const QVector<Waypoint *> &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<Waypoint*,WaypointView*> viewIt(wpViews);
viewIt.toFront();
while(viewIt.hasNext())
{ {
WaypointView* widget = wpViews.find(waypoints[i]).value(); viewIt.next();
Waypoint *cur = viewIt.key();
if (waypoints[i]->getId() == seq) int i;
for (i = 0; i < waypoints.size(); i++)
{ {
waypoints[i]->setCurrent(true); if (waypoints[i] == cur)
widget->setCurrent(true); {
break;
}
} }
else if (i == waypoints.size())
{ {
waypoints[i]->setCurrent(false); WaypointView* widget = wpViews.find(cur).value();
widget->setCurrent(false); 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() ...@@ -211,21 +226,16 @@ void WaypointList::read()
{ {
if (uas) if (uas)
{ {
QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList(); uas->getWaypointManager().readWaypoints();
while(waypoints.size()>0)
{
removeWaypoint(waypoints[0]);
}
emit requestWaypoints();
} }
} }
void WaypointList::transmit() void WaypointList::transmit()
{ {
emit sendWaypoints(); if (uas)
//emit requestWaypoints(); FIXME {
uas->getWaypointManager().writeWaypoints();
}
} }
void WaypointList::add() void WaypointList::add()
...@@ -233,16 +243,17 @@ void WaypointList::add() ...@@ -233,16 +243,17 @@ void WaypointList::add()
// Only add waypoints if UAS is present // Only add waypoints if UAS is present
if (uas) if (uas)
{ {
QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList(); const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
if (waypoints.size() > 0) if (waypoints.size() > 0)
{ {
Waypoint *last = waypoints.at(waypoints.size()-1); 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 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() ...@@ -251,119 +262,61 @@ void WaypointList::addCurrentPositonWaypoint()
{ {
if (uas) if (uas)
{ {
QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList(); const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
if (waypoints.size() > 0) if (waypoints.size() > 0)
{ {
Waypoint *last = waypoints.at(waypoints.size()-1); 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 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) if (uas)
{ {
uas->getWaypointManager().getWaypointList().push_back(wp); const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
if (!wpViews.contains(wp))
//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); if (waypoints[i] == wp)
wpViews.insert(wp, wpview); break;
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)));
} }
}
}
void WaypointList::redrawList()
{
if (uas)
{
QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
// Clear list layout // if wp was found and its not the first entry, move it
if (!wpViews.empty()) if (i < waypoints.size() && i > 0)
{ {
QMapIterator<Waypoint*,WaypointView*> viewIt(wpViews); uas->getWaypointManager().localMoveWaypoint(i, i-1);
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) void WaypointList::moveDown(Waypoint* wp)
{ {
if (uas) if (uas)
{ {
QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList(); const QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
int id = wp->getId(); //get the current position of wp in the local storage
if (waypoints.size() > 1 && waypoints.size() > id) int i;
for (i = 0; i < waypoints.size(); i++)
{ {
Waypoint* temp = waypoints[id]; if (waypoints[i] == wp)
if (id > 0) break;
{
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();
} }
}
}
void WaypointList::moveDown(Waypoint* wp) // if wp was found and its not the last entry, move it
{ if (i < waypoints.size()-1)
if (uas)
{
QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList();
int id = wp->getId();
if (waypoints.size() > 1 && waypoints.size() > id)
{ {
Waypoint* temp = waypoints[id]; uas->getWaypointManager().localMoveWaypoint(i, i+1);
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();
} }
} }
} }
...@@ -372,24 +325,7 @@ void WaypointList::removeWaypoint(Waypoint* wp) ...@@ -372,24 +325,7 @@ void WaypointList::removeWaypoint(Waypoint* wp)
{ {
if (uas) if (uas)
{ {
QVector<Waypoint *> &waypoints = uas->getWaypointManager().getWaypointList(); uas->getWaypointManager().localRemoveWaypoint(wp->getId());
// 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;
}
} }
} }
...@@ -409,20 +345,7 @@ void WaypointList::saveWaypoints() ...@@ -409,20 +345,7 @@ void WaypointList::saveWaypoints()
if (uas) if (uas)
{ {
QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)")); QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)"));
QFile file(fileName); uas->getWaypointManager().localSaveWaypoints(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();
} }
} }
...@@ -431,25 +354,7 @@ void WaypointList::loadWaypoints() ...@@ -431,25 +354,7 @@ void WaypointList::loadWaypoints()
if (uas) if (uas)
{ {
QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)")); QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)"));
QFile file(fileName); uas->getWaypointManager().localLoadWaypoints(fileName);
if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
return;
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();
} }
} }
src/ui/WaypointList.h
View file @ 11fa19c3
...@@ -55,7 +55,6 @@ class WaypointList : public QWidget { ...@@ -55,7 +55,6 @@ class WaypointList : public QWidget {
public slots: public slots:
void setUAS(UASInterface* uas); void setUAS(UASInterface* uas);
void redrawList();
//UI Buttons //UI Buttons
void saveWaypoints(); void saveWaypoints();
...@@ -73,23 +72,18 @@ public slots: ...@@ -73,23 +72,18 @@ public slots:
void updateStatusLabel(const QString &string); void updateStatusLabel(const QString &string);
void changeCurrentWaypoint(quint16 seq); ///< The user wants to change the current waypoint void changeCurrentWaypoint(quint16 seq); ///< The user wants to change the current waypoint
void currentWaypointChanged(quint16 seq); ///< The waypointplanner changed the current waypoint void currentWaypointChanged(quint16 seq); ///< The waypoint planner 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 waypointListChanged(void); ///< The waypoint manager informs that the waypoint list was changed
void addWaypoint(Waypoint* wp);
void removeWaypoint(Waypoint* wp); void removeWaypoint(Waypoint* wp);
void waypointReached(quint16 waypointId); void waypointReached(quint16 waypointId);
void updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec); void updateLocalPosition(UASInterface*, double x, double y, double z, quint64 usec);
void updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64 usec); void updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64 usec);
signals:
void sendWaypoints();
void requestWaypoints();
void clearWaypointList();
void setCurrent(quint16);
protected: protected:
virtual void changeEvent(QEvent *e); virtual void changeEvent(QEvent *e);
protected:
QMap<Waypoint*, WaypointView*> wpViews; QMap<Waypoint*, WaypointView*> wpViews;
QVBoxLayout* listLayout; QVBoxLayout* listLayout;
UASInterface* uas; UASInterface* uas;
......
src/ui/WaypointView.cc
View file @ 11fa19c3
...@@ -48,15 +48,7 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) : ...@@ -48,15 +48,7 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
this->wp = wp; this->wp = wp;
// Read values and set user interface // Read values and set user interface
m_ui->xSpinBox->setValue(wp->getX()); updateValues();
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());
connect(m_ui->xSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double))); connect(m_ui->xSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setX(double)));
connect(m_ui->ySpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double))); connect(m_ui->ySpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setY(double)));
...@@ -110,15 +102,30 @@ void WaypointView::changedCurrent(int state) ...@@ -110,15 +102,30 @@ void WaypointView::changedCurrent(int state)
{ {
if (state == 0) if (state == 0)
{ {
wp->setCurrent(false); //m_ui->selectedBox->setChecked(true);
//m_ui->selectedBox->setCheckState(Qt::Checked);
//wp->setCurrent(false);
} }
else else
{ {
wp->setCurrent(true); //wp->setCurrent(true);
emit changeCurrentWaypoint(wp->getId()); //the slot changeCurrentWaypoint() in WaypointList sets all other current flags to false 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) void WaypointView::setCurrent(bool state)
{ {
if (state) if (state)
......
src/ui/WaypointView.h
View file @ 11fa19c3
...@@ -57,6 +57,7 @@ public slots: ...@@ -57,6 +57,7 @@ public slots:
void remove(); void remove();
void changedAutoContinue(int); void changedAutoContinue(int);
void changedCurrent(int); void changedCurrent(int);
void updateValues(void);
void setYaw(int); //hidden degree to radian conversion void setYaw(int); //hidden degree to radian conversion
......
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