From 7714d153669754bbc4e36fdd610f353cb399a815 Mon Sep 17 00:00:00 2001
From: pixhawk <mavteam@student.ethz.ch>
Date: Wed, 16 Jun 2010 18:20:06 +0200
Subject: [PATCH 1/3] removed debug messages
---
src/uas/UAS.cc | 2 +-
src/uas/UASWaypointManager.cc | 2 --
src/ui/WaypointList.cc | 9 +++++----
3 files changed, 6 insertions(+), 7 deletions(-)
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 19b1686..b313578 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -428,7 +428,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
mavlink_waypoint_t wp;
mavlink_msg_waypoint_decode(&message, &wp);
- qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z;
+ //qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z;
if(wp.target_system == mavlink->getSystemId() && wp.target_component == mavlink->getComponentId())
{
waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc
index 8784eb3..03ad97f 100644
--- a/src/uas/UASWaypointManager.cc
+++ b/src/uas/UASWaypointManager.cc
@@ -37,8 +37,6 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
{
if (systemId == current_partner_systemid && compId == current_partner_compid && current_state == WP_GETLIST_GETWPS && wp->seq == current_wp_id)
{
- qDebug() << "got waypoint (" << wp->seq << ") from ID " << systemId << " x=" << wp->x << " y=" << wp->y << " z=" << wp->z;
-
if(wp->seq == current_wp_id)
{
//update the UI FIXME
diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc
index 5a7d420..eef95b0 100644
--- a/src/ui/WaypointList.cc
+++ b/src/ui/WaypointList.cc
@@ -159,8 +159,7 @@ void WaypointList::read()
{
removeWaypoint(waypoints[0]);
}
-
- redrawList();
+
emit requestWaypoints();
}
@@ -170,6 +169,7 @@ void WaypointList::transmit()
m_ui->transmitButton->setEnabled(false);
emit sendWaypoints(waypoints);
+ //emit requestWaypoints(); FIXME
}
void WaypointList::add()
@@ -179,17 +179,18 @@ void WaypointList::add()
{
if (waypoints.size() > 0)
{
- addWaypoint(new Waypoint(waypoints.size(), 0.0, 0.1, -0.5, 0.0, false, false));
+ addWaypoint(new Waypoint(waypoints.size(), 0.0, 0.0, -0.0, 0.0, false, false));
}
else
{
- addWaypoint(new Waypoint(waypoints.size(), 0.0, 0.0, -0.5, 360.0, false, true));
+ addWaypoint(new Waypoint(waypoints.size(), 0.0, 0.0, -0.0, 0.0, false, true));
}
}
}
void WaypointList::addWaypoint(Waypoint* wp)
{
+ waypoints.push_back(wp);
if (!wpViews.contains(wp))
{
WaypointView* wpview = new WaypointView(wp, this);
From d09ed967537efe4e03dbd56c2c969e22a1bdffc2 Mon Sep 17 00:00:00 2001
From: pixhawk <pixhawk@student.ethz.ch>
Date: Wed, 16 Jun 2010 20:48:02 +0200
Subject: [PATCH 2/3] Added a few new transformations
---
src/comm/MAVLinkProtocol.cc | 5 ++++-
src/uas/UAS.cc | 8 ++++----
src/ui/HSIDisplay.cc | 16 ++++++++++++----
src/ui/HSIDisplay.h | 4 +++-
4 files changed, 23 insertions(+), 10 deletions(-)
diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
index c992411..215a4a6 100644
--- a/src/comm/MAVLinkProtocol.cc
+++ b/src/comm/MAVLinkProtocol.cc
@@ -49,6 +49,7 @@ This file is part of the PIXHAWK project
#include "configuration.h"
#include "LinkManager.h"
#include <mavlink.h>
+#include "QGC.h"
/**
* The default constructor will create a new MAVLink object sending heartbeats at
@@ -129,7 +130,9 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link)
if (m_loggingEnabled)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
- mavlink_msg_to_send_buffer(buf, &message);
+ quint64 time = MG::TIME::getGroundTimeNowUsecs();
+ memcpy(buf, (void*)&time, sizeof(quint64));
+ mavlink_msg_to_send_buffer(buf+sizeof(quint64), &message);
m_logfile->write((const char*) buf);
qDebug() << "WROTE LOGFILE";
}
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index b313578..e745010 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -406,11 +406,11 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
mavlink_position_controller_output_t out;
mavlink_msg_position_controller_output_decode(&message, &out);
- quint64 time = MG::TIME::getGroundTimeNowUsecs();
+ quint64 time = MG::TIME::getGroundTimeNow();
//emit positionSetPointsChanged(uasId, out.x/127.0f, out.y/127.0f, out.z/127.0f, out.yaw, time);
- emit valueChanged(uasId, "pos control x", out.x, time/1000.0f);
- emit valueChanged(uasId, "pos control y", out.y, time/1000.0f);
- emit valueChanged(uasId, "pos control z", out.z, time/1000.0f);
+ emit valueChanged(uasId, "pos control x", out.x, time);
+ emit valueChanged(uasId, "pos control y", out.y, time);
+ emit valueChanged(uasId, "pos control z", out.z, time);
}
break;
case MAVLINK_MSG_ID_WAYPOINT_COUNT:
diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc
index 9db24bd..ff6adf1 100644
--- a/src/ui/HSIDisplay.cc
+++ b/src/ui/HSIDisplay.cc
@@ -210,7 +210,7 @@ void HSIDisplay::paintDisplay()
// Transform from body to world coordinates
m = metricWorldToBody(m);
// Scale from metric body to screen reference units
- QPointF s = metricBodyToRefX(m);
+ QPointF s = metricBodyToRef(m);
drawLine(s.x(), s.y(), xCenterPos, yCenterPos, 1.5f, QGC::ColorCyan, &painter);
}
@@ -297,11 +297,19 @@ QPointF HSIDisplay::refToMetricBody(QPointF &ref)
/**
* @see refToScreenX()
*/
-QPointF HSIDisplay::metricBodyToRefX(QPointF &metric)
+QPointF HSIDisplay::metricBodyToRef(QPointF &metric)
{
return QPointF(((metric.y())/ metricWidth) * vwidth + xCenterPos, ((-metric.x()) / metricWidth) * vwidth + yCenterPos);
}
+QPointF HSIDisplay::metricBodyToScreen(QPointF metric)
+{
+ QPointF ref = metricBodyToRef(metric);
+ ref.setX(refToScreenX(ref.x()));
+ ref.setY(refToScreenY(ref.y()));
+ return ref;
+}
+
void HSIDisplay::mouseDoubleClickEvent(QMouseEvent * event)
{
static bool dragStarted;
@@ -508,7 +516,7 @@ void HSIDisplay::drawSetpointXY(float x, float y, float yaw, const QColor &color
// Transform from body to world coordinates
in = metricWorldToBody(in);
// Scale from metric to screen reference coordinates
- QPointF p = metricBodyToRefX(in);
+ QPointF p = metricBodyToRef(in);
drawCircle(p.x(), p.y(), radius, 0.4f, color, &painter);
radius *= 0.8;
drawLine(p.x(), p.y(), p.x()+sin(yaw) * radius, p.y()-cos(yaw) * radius, refLineWidthToPen(0.4f), color, &painter);
@@ -522,7 +530,7 @@ void HSIDisplay::drawSafetyArea(const QPointF &topLeft, const QPointF &bottomRig
pen.setWidthF(refLineWidthToPen(0.1f));
pen.setColor(color);
painter.setPen(pen);
- painter.drawRect(QRectF(topLeft, bottomRight));
+ painter.drawRect(QRectF(metricBodyToScreen(metricWorldToBody(topLeft)), metricBodyToScreen(metricWorldToBody(bottomRight))));
}
void HSIDisplay::drawGPS(QPainter &painter)
diff --git a/src/ui/HSIDisplay.h b/src/ui/HSIDisplay.h
index 50de3f5..20b7ba3 100644
--- a/src/ui/HSIDisplay.h
+++ b/src/ui/HSIDisplay.h
@@ -98,7 +98,9 @@ protected:
/** @brief Reference coordinates to metric coordinates */
QPointF refToMetricBody(QPointF &ref);
/** @brief Metric coordinates to reference coordinates */
- QPointF metricBodyToRefX(QPointF &metric);
+ QPointF metricBodyToRef(QPointF &metric);
+ /** @brief Metric body coordinates to screen coordinates */
+ QPointF metricBodyToScreen(QPointF metric);
/**
* @brief Private data container class to be used within the HSI widget
From 55ae8c09607b01a7c3d0338d29cde8e7c03975d2 Mon Sep 17 00:00:00 2001
From: pixhawk <mavteam@student.ethz.ch>
Date: Thu, 17 Jun 2010 15:40:17 +0200
Subject: [PATCH 3/3] waypoints working
---
src/uas/UAS.cc | 16 ++++++--
src/uas/UASWaypointManager.cc | 86 +++++++++++++++++++++++++++++++++++++------
src/uas/UASWaypointManager.h | 42 +++++++++++++++++++--
src/ui/WaypointList.cc | 85 ++++++++++++------------------------------
src/ui/WaypointList.h | 18 ++++-----
src/ui/WaypointView.cc | 45 ++++++++++++++++------
src/ui/WaypointView.h | 14 +++----
src/ui/WaypointView.ui | 64 +++++++++++++++++++++++++++++---
8 files changed, 255 insertions(+), 115 deletions(-)
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index e745010..230a2b5 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -449,11 +449,20 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
case MAVLINK_MSG_ID_WAYPOINT_REACHED:
{
-// mavlink_waypoint_reached_t wp;
-// mavlink_msg_waypoint_reached_decode(&message, &wp);
-// emit waypointReached(this, wp.id);
+ mavlink_waypoint_reached_t wpr;
+ mavlink_msg_waypoint_reached_decode(&message, &wpr);
+ waypointManager.handleWaypointReached(message.sysid, message.compid, &wpr);
}
break;
+
+ case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
+ {
+ mavlink_waypoint_set_current_t wpsc;
+ mavlink_msg_waypoint_set_current_decode(&message, &wpsc);
+ waypointManager.handleWaypointSetCurrent(message.sysid, message.compid, &wpsc);
+ }
+ break;
+
case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT:
{
mavlink_local_position_setpoint_t p;
@@ -461,6 +470,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs());
}
break;
+
case MAVLINK_MSG_ID_STATUSTEXT:
{
QByteArray b;
diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc
index 03ad97f..4f5b9d9 100644
--- a/src/uas/UASWaypointManager.cc
+++ b/src/uas/UASWaypointManager.cc
@@ -1,18 +1,70 @@
+/*=====================================================================
+
+PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
+
+(c) 2009, 2010 PIXHAWK PROJECT <http://pixhawk.ethz.ch>
+
+This file is part of the PIXHAWK project
+
+ PIXHAWK is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PIXHAWK is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ * @brief Implementation of the waypoint protocol handler
+ *
+ * @author Petri Tanskanen <mavteam@student.ethz.ch>
+ *
+ */
+
#include "UASWaypointManager.h"
#include "UAS.h"
+#define PROTOCOL_TIMEOUT_MS 2000
+
UASWaypointManager::UASWaypointManager(UAS &_uas)
: uas(_uas),
current_wp_id(0),
current_count(0),
current_state(WP_IDLE),
current_partner_systemid(0),
- current_partner_compid(0)
+ current_partner_compid(0),
+ protocol_timer(this)
+{
+ connect(&protocol_timer, SIGNAL(timeout()), this, SLOT(timeout()));
+}
+
+void UASWaypointManager::timeout()
{
+ protocol_timer.stop();
+
+ qDebug() << "Waypoint transaction (state=" << current_state << ") timed out going to state WP_IDLE";
+
+ emit updateStatusString("Operation timed out.");
+
+ current_state = WP_IDLE;
+ current_count = 0;
+ current_wp_id = 0;
+ current_partner_systemid = 0;
+ current_partner_compid = 0;
}
void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, quint16 count)
{
+ protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+
if (current_state == WP_GETLIST && systemId == current_partner_systemid && compId == current_partner_compid)
{
qDebug() << "got waypoint count (" << count << ") from ID " << systemId;
@@ -35,6 +87,8 @@ void UASWaypointManager::handleWaypointCount(quint8 systemId, quint8 compId, qui
void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp)
{
+ protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+
if (systemId == current_partner_systemid && compId == current_partner_compid && current_state == WP_GETLIST_GETWPS && wp->seq == current_wp_id)
{
if(wp->seq == current_wp_id)
@@ -58,6 +112,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
current_partner_systemid = 0;
current_partner_compid = 0;
+ protocol_timer.stop();
emit updateStatusString("done.");
qDebug() << "got all waypoints from ID " << systemId;
@@ -72,6 +127,8 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr)
{
+ protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+
if (systemId == current_partner_systemid && compId == current_partner_compid && ((current_state == WP_SENDLIST && wpr->seq == 0) || (current_state == WP_SENDLIST_SENDWPS && (wpr->seq == current_wp_id || wpr->seq == current_wp_id + 1)) || (current_state == WP_IDLE && wpr->seq == current_count-1)))
{
qDebug() << "handleWaypointRequest";
@@ -87,6 +144,7 @@ void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, m
//all waypoints sent, but we still have to wait for a possible rerequest of the last waypoint
current_state = WP_IDLE;
+ protocol_timer.stop();
emit updateStatusString("done.");
qDebug() << "send all waypoints to ID " << systemId;
@@ -99,17 +157,24 @@ void UASWaypointManager::handleWaypointRequest(quint8 systemId, quint8 compId, m
}
}
-void UASWaypointManager::clearWaypointList()
+void UASWaypointManager::handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr)
{
-
+ if (systemId == uas.getUASID() && compId == MAV_COMP_ID_WAYPOINTPLANNER)
+ {
+ emit updateStatusString(QString("Reached waypoint %1").arg(wpr->seq));
+ }
}
-void UASWaypointManager::currentWaypointChanged(quint16)
+void UASWaypointManager::handleWaypointSetCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_set_current_t *wpr)
{
-
+ if (systemId == uas.getUASID() && compId == MAV_COMP_ID_WAYPOINTPLANNER)
+ {
+ qDebug() << "new current waypoint" << wpr->seq;
+ emit currentWaypointChanged(wpr->seq);
+ }
}
-void UASWaypointManager::removeWaypointId(quint16)
+void UASWaypointManager::clearWaypointList()
{
}
@@ -118,6 +183,8 @@ void UASWaypointManager::requestWaypoints()
{
if(current_state == WP_IDLE)
{
+ protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+
mavlink_message_t message;
mavlink_waypoint_request_list_t wprl;
@@ -145,6 +212,8 @@ void UASWaypointManager::sendWaypoints(const QVector<Waypoint*> &list)
{
if (list.count() > 0)
{
+ protocol_timer.start(PROTOCOL_TIMEOUT_MS);
+
current_count = list.count();
current_state = WP_SENDLIST;
current_wp_id = 0;
@@ -240,8 +309,3 @@ void UASWaypointManager::sendWaypoint(quint16 seq)
qDebug() << "sent waypoint (" << wp->seq << ") to ID " << wp->target_system;
}
}
-
-void UASWaypointManager::waypointChanged(Waypoint*)
-{
-
-}
diff --git a/src/uas/UASWaypointManager.h b/src/uas/UASWaypointManager.h
index b5cad17..247feac 100644
--- a/src/uas/UASWaypointManager.h
+++ b/src/uas/UASWaypointManager.h
@@ -1,8 +1,40 @@
+/*=====================================================================
+
+PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
+
+(c) 2009, 2010 PIXHAWK PROJECT <http://pixhawk.ethz.ch>
+
+This file is part of the PIXHAWK project
+
+ PIXHAWK is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ PIXHAWK is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
+
+======================================================================*/
+
+/**
+ * @file
+ * @brief Definition of the waypoint protocol handler
+ *
+ * @author Petri Tanskanen <mavteam@student.ethz.ch>
+ *
+ */
+
#ifndef UASWAYPOINTMANAGER_H
#define UASWAYPOINTMANAGER_H
#include <QObject>
#include <QVector>
+#include <QTimer>
#include "Waypoint.h"
#include <mavlink.h>
class UAS;
@@ -25,22 +57,23 @@ public:
void handleWaypointCount(quint8 systemId, quint8 compId, quint16 count);
void handleWaypoint(quint8 systemId, quint8 compId, mavlink_waypoint_t *wp);
void handleWaypointRequest(quint8 systemId, quint8 compId, mavlink_waypoint_request_t *wpr);
+ void handleWaypointReached(quint8 systemId, quint8 compId, mavlink_waypoint_reached_t *wpr);
+ void handleWaypointSetCurrent(quint8 systemId, quint8 compId, mavlink_waypoint_set_current_t *wpr);
private:
void sendWaypointRequest(quint16 seq);
void sendWaypoint(quint16 seq);
public slots:
+ void timeout();
void clearWaypointList();
- void currentWaypointChanged(quint16);
- void removeWaypointId(quint16);
void requestWaypoints();
void sendWaypoints(const QVector<Waypoint *> &list);
- void waypointChanged(Waypoint*);
signals:
void waypointUpdated(int,quint16,double,double,double,double,bool,bool); ///< Adds a waypoint to the waypoint list widget
- void updateStatusString(const QString &); ///< updates the current status string
+ 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
@@ -51,6 +84,7 @@ private:
quint8 current_partner_compid; ///< The current protocol communication target component
QVector<mavlink_waypoint_t *> waypoint_buffer; ///< communication buffer for waypoints
+ QTimer protocol_timer; ///< Timer to catch timeouts
};
#endif // UASWAYPOINTMANAGER_H
diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc
index eef95b0..89bde86 100644
--- a/src/ui/WaypointList.cc
+++ b/src/ui/WaypointList.cc
@@ -45,8 +45,6 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
{
m_ui->setupUi(this);
- transmitDelay = new QTimer(this);
-
listLayout = new QVBoxLayout(m_ui->listWidget);
listLayout->setSpacing(6);
listLayout->setMargin(0);
@@ -73,7 +71,6 @@ WaypointList::WaypointList(QWidget *parent, UASInterface* uas) :
connect(m_ui->loadButton, SIGNAL(clicked()), this, SLOT(loadWaypoints()));
connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*)));
- connect(transmitDelay, SIGNAL(timeout()), this, SLOT(reenableTransmit()));
// STATUS LABEL
updateStatusLabel("");
@@ -97,14 +94,14 @@ void WaypointList::setUAS(UASInterface* uas)
if (this->uas == NULL && uas != NULL)
{
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)), this, SLOT(setWaypoint(int,quint16,double,double,double,double,bool,bool)));
- //connect(this, SIGNAL(waypointChanged(Waypoint*)), &uas->getWaypointManager(), SLOT(setWaypoint(Waypoint*)));
- //connect(this, SIGNAL(currentWaypointChanged(int)), &uas->getWaypointManager(), SLOT(setWaypointActive(quint16)));
+ connect(&uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
+
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(&uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &)));
}
}
@@ -112,7 +109,6 @@ void WaypointList::setWaypoint(int uasId, quint16 id, double x, double y, double
{
if (uasId == this->uas->getUASID())
{
- transmitDelay->start(1000);
Waypoint* wp = new Waypoint(id, x, y, z, yaw, autocontinue, current);
addWaypoint(wp);
}
@@ -123,34 +119,30 @@ void WaypointList::waypointReached(UASInterface* uas, quint16 waypointId)
Q_UNUSED(uas);
qDebug() << "Waypoint reached: " << waypointId;
- /*if (waypoints.size() > waypointId)
+ updateStatusLabel(QString("Waypoint %1 reached.").arg(waypointId));
+}
+
+void WaypointList::currentWaypointChanged(quint16 seq)
+{
+ if (seq < waypoints.size())
{
- if (waypoints[waypointId]->autocontinue == true)
+ for(int i = 0; i < waypoints.size(); i++)
{
+ WaypointView* widget = wpViews.find(waypoints[i]).value();
- for(int i = 0; i < waypoints.size(); i++)
+ if (waypoints[i]->getId() == seq)
{
- if (i == waypointId+1)
- {
- waypoints[i]->current = true;
- WaypointView* widget = wpViews.find(waypoints[i]).value();
- widget->setCurrent();
- }
- else
- {
- if (waypoints[i]->current)
- {
- waypoints[i]->current = false;
- WaypointView* widget = wpViews.find(waypoints[i]).value();
- widget->removeCurrentCheck();
- }
- }
+ waypoints[i]->setCurrent(true);
+ widget->setCurrent(true);
+ }
+ else
+ {
+ waypoints[i]->setCurrent(false);
+ widget->setCurrent(false);
}
- redrawList();
-
- qDebug() << "NEW WAYPOINT SET";
}
- }*/
+ redrawList();
+ }
}
void WaypointList::read()
@@ -165,9 +157,6 @@ void WaypointList::read()
void WaypointList::transmit()
{
- transmitDelay->start(1000);
- m_ui->transmitButton->setEnabled(false);
-
emit sendWaypoints(waypoints);
//emit requestWaypoints(); FIXME
}
@@ -199,8 +188,7 @@ void WaypointList::addWaypoint(Waypoint* 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(setCurrentWaypoint(Waypoint*)), this, SLOT(setCurrentWaypoint(Waypoint*)));
- //connect(wpview, SIGNAL(waypointUpdated(Waypoint*)), this, SIGNAL(waypointChanged(Waypoint*)));
+ connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16)));
}
}
@@ -292,28 +280,6 @@ void WaypointList::removeWaypoint(Waypoint* wp)
}
}
-void WaypointList::setCurrentWaypoint(Waypoint* wp)
-{
- for(int i = 0; i < waypoints.size(); i++)
- {
- if (waypoints[i] == wp)
- {
- waypoints[i]->setCurrent(true);
- // Retransmit waypoint
- //uas->getWaypointManager().setWaypointActive(i);
- }
- else
- {
- if (waypoints[i]->getCurrent())
- {
- waypoints[i]->setCurrent(false);
- WaypointView* widget = wpViews.find(waypoints[i]).value();
- widget->removeCurrentCheck();
- }
- }
- }
-}
-
void WaypointList::changeEvent(QEvent *e)
{
switch (e->type()) {
@@ -325,11 +291,6 @@ void WaypointList::changeEvent(QEvent *e)
}
}
-void WaypointList::reenableTransmit()
-{
- m_ui->transmitButton->setEnabled(true);
-}
-
void WaypointList::saveWaypoints()
{
QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)"));
diff --git a/src/ui/WaypointList.h b/src/ui/WaypointList.h
index 8aeef71..fecd118 100644
--- a/src/ui/WaypointList.h
+++ b/src/ui/WaypointList.h
@@ -56,7 +56,6 @@ class WaypointList : public QWidget {
public slots:
void setUAS(UASInterface* uas);
void redrawList();
- void reenableTransmit();
//UI Buttons
void saveWaypoints();
@@ -66,36 +65,33 @@ public slots:
void add();
void moveUp(Waypoint* wp);
void moveDown(Waypoint* wp);
- void setCurrentWaypoint(Waypoint* wp);
/** @brief sets statusLabel string */
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);
void addWaypoint(Waypoint* wp);
void removeWaypoint(Waypoint* wp);
void waypointReached(UASInterface* uas, quint16 waypointId);
+signals:
+ void sendWaypoints(const QVector<Waypoint*> &);
+ void requestWaypoints();
+ void clearWaypointList();
+
protected:
virtual void changeEvent(QEvent *e);
QVector<Waypoint*> waypoints;
QMap<Waypoint*, WaypointView*> wpViews;
QVBoxLayout* listLayout;
- QTimer* transmitDelay;
UASInterface* uas;
private:
Ui::WaypointList *m_ui;
-
-signals:
- //void waypointChanged(Waypoint*);
- //void currentWaypointChanged(int);
- //void removeWaypointId(int);
- void sendWaypoints(const QVector<Waypoint*> &);
- void requestWaypoints();
- void clearWaypointList();
};
#endif // WAYPOINTLIST_H
diff --git a/src/ui/WaypointView.cc b/src/ui/WaypointView.cc
index 36acc29..17e0919 100644
--- a/src/ui/WaypointView.cc
+++ b/src/ui/WaypointView.cc
@@ -34,6 +34,8 @@ This file is part of the PIXHAWK project
#include <QDoubleSpinBox>
#include <QDebug>
+#include <cmath> //M_PI
+
#include "WaypointView.h"
#include "ui_WaypointView.h"
@@ -49,7 +51,7 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
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_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()));
@@ -57,14 +59,22 @@ WaypointView::WaypointView(Waypoint* wp, QWidget* parent) :
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->zSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setZ(double)));
- connect(m_ui->yawSpinBox, SIGNAL(valueChanged(double)), wp, SLOT(setYaw(double)));
+
+ //hidden degree to radian conversion of the yaw angle
+ connect(m_ui->yawSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setYaw(int)));
+ connect(this, SIGNAL(setYaw(double)), wp, SLOT(setYaw(double)));
connect(m_ui->upButton, SIGNAL(clicked()), this, SLOT(moveUp()));
connect(m_ui->downButton, SIGNAL(clicked()), this, SLOT(moveDown()));
connect(m_ui->removeButton, SIGNAL(clicked()), this, SLOT(remove()));
- connect(m_ui->autoContinue, SIGNAL(stateChanged(int)), this, SLOT(setAutoContinue(int)));
- connect(m_ui->selectedBox, SIGNAL(clicked()), this, SLOT(setCurrent()));
+ connect(m_ui->autoContinue, SIGNAL(stateChanged(int)), this, SLOT(changedAutoContinue(int)));
+ connect(m_ui->selectedBox, SIGNAL(stateChanged(int)), this, SLOT(changedCurrent(int)));
+}
+
+void WaypointView::setYaw(int yawDegree)
+{
+ emit setYaw((double)yawDegree*M_PI/180.);
}
void WaypointView::moveUp()
@@ -83,26 +93,37 @@ void WaypointView::remove()
delete this;
}
-void WaypointView::setAutoContinue(int state)
+void WaypointView::changedAutoContinue(int state)
{
if (state == 0)
wp->setAutocontinue(false);
else
wp->setAutocontinue(true);
- emit waypointUpdated(wp);
}
-void WaypointView::removeCurrentCheck()
+void WaypointView::changedCurrent(int state)
{
- m_ui->selectedBox->setCheckState(Qt::Unchecked);
+ if (state == 0)
+ {
+ wp->setCurrent(false);
+ }
+ else
+ {
+ wp->setCurrent(true);
+ emit currentWaypointChanged(wp->getId()); //the slot currentWayppointChanged() in WaypointList sets all other current flags to false
+ }
}
-void WaypointView::setCurrent()
+void WaypointView::setCurrent(bool state)
{
- if (m_ui->selectedBox->isChecked())
- emit setCurrentWaypoint(wp);
- else
+ if (state)
+ {
m_ui->selectedBox->setCheckState(Qt::Checked);
+ }
+ else
+ {
+ m_ui->selectedBox->setCheckState(Qt::Unchecked);
+ }
}
WaypointView::~WaypointView()
diff --git a/src/ui/WaypointView.h b/src/ui/WaypointView.h
index d10f8d1..bf4fb1b 100644
--- a/src/ui/WaypointView.h
+++ b/src/ui/WaypointView.h
@@ -49,15 +49,16 @@ class WaypointView : public QWidget {
virtual ~WaypointView();
public:
- void removeCurrentCheck();
+ void setCurrent(bool state);
public slots:
void moveUp();
void moveDown();
void remove();
- void setAutoContinue(int);
- void setCurrent();
- //void setText();
+ void changedAutoContinue(int);
+ void changedCurrent(int);
+
+ void setYaw(int); //hidden degree to radian conversion
protected:
virtual void changeEvent(QEvent *e);
@@ -70,9 +71,8 @@ signals:
void moveUpWaypoint(Waypoint*);
void moveDownWaypoint(Waypoint*);
void removeWaypoint(Waypoint*);
- void setCurrentWaypoint(Waypoint*);
- void setWaypointText(Waypoint*, QString);
- void waypointUpdated(Waypoint*);
+ void currentWaypointChanged(quint16);
+ void setYaw(double);
};
#endif // WAYPOINTVIEW_H
diff --git a/src/ui/WaypointView.ui b/src/ui/WaypointView.ui
index 32d847b..e344c56 100644
--- a/src/ui/WaypointView.ui
+++ b/src/ui/WaypointView.ui
@@ -180,9 +180,21 @@ QProgressBar::chunk#thrustBar {
</item>
<item>
<widget class="QLabel" name="idLabel">
+ <property name="minimumSize">
+ <size>
+ <width>15</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="toolTip">
+ <string>Waypoint Sequence Number</string>
+ </property>
<property name="text">
<string>TextLabel</string>
</property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
</widget>
</item>
<item>
@@ -200,22 +212,40 @@ QProgressBar::chunk#thrustBar {
</item>
<item>
<widget class="QDoubleSpinBox" name="xSpinBox">
+ <property name="toolTip">
+ <string>Position X coordinate</string>
+ </property>
+ <property name="suffix">
+ <string>m</string>
+ </property>
<property name="minimum">
<double>-100000.000000000000000</double>
</property>
<property name="maximum">
<double>100000.000000000000000</double>
</property>
+ <property name="singleStep">
+ <double>0.050000000000000</double>
+ </property>
</widget>
</item>
<item>
<widget class="QDoubleSpinBox" name="ySpinBox">
+ <property name="toolTip">
+ <string>Position Y coordinate</string>
+ </property>
+ <property name="suffix">
+ <string>m</string>
+ </property>
<property name="minimum">
<double>-100000.000000000000000</double>
</property>
<property name="maximum">
<double>100000.000000000000000</double>
</property>
+ <property name="singleStep">
+ <double>0.050000000000000</double>
+ </property>
<property name="value">
<double>0.000000000000000</double>
</property>
@@ -223,21 +253,36 @@ QProgressBar::chunk#thrustBar {
</item>
<item>
<widget class="QDoubleSpinBox" name="zSpinBox">
+ <property name="toolTip">
+ <string>Position Z coordinate</string>
+ </property>
+ <property name="suffix">
+ <string>m</string>
+ </property>
<property name="minimum">
<double>-100000.000000000000000</double>
</property>
<property name="maximum">
- <double>100000.000000000000000</double>
+ <double>0.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.050000000000000</double>
</property>
</widget>
</item>
<item>
- <widget class="QDoubleSpinBox" name="yawSpinBox">
- <property name="minimum">
- <double>0.000000000000000</double>
+ <widget class="QSpinBox" name="yawSpinBox">
+ <property name="toolTip">
+ <string>Yaw angle</string>
+ </property>
+ <property name="wrapping">
+ <bool>true</bool>
+ </property>
+ <property name="suffix">
+ <string>°</string>
</property>
<property name="maximum">
- <double>360.000000000000000</double>
+ <number>359</number>
</property>
</widget>
</item>
@@ -259,6 +304,9 @@ QProgressBar::chunk#thrustBar {
<height>22</height>
</size>
</property>
+ <property name="toolTip">
+ <string>Move Up</string>
+ </property>
<property name="text">
<string/>
</property>
@@ -276,6 +324,9 @@ QProgressBar::chunk#thrustBar {
<height>22</height>
</size>
</property>
+ <property name="toolTip">
+ <string>Move Down</string>
+ </property>
<property name="text">
<string/>
</property>
@@ -293,6 +344,9 @@ QProgressBar::chunk#thrustBar {
<height>22</height>
</size>
</property>
+ <property name="toolTip">
+ <string>Delete</string>
+ </property>
<property name="text">
<string/>
</property>