diff --git a/src/QGC.cc b/src/QGC.cc
index c30209f..50eb1c0 100644
--- a/src/QGC.cc
+++ b/src/QGC.cc
@@ -47,6 +47,15 @@ quint64 groundTimeMilliseconds()
return static_cast<quint64>(seconds + (time.time().msec()));
}
+qreal groundTimeSeconds()
+{
+ QDateTime time = QDateTime::currentDateTime();
+ time = time.toUTC();
+ /* Return time in seconds unit */
+ quint64 seconds = time.toTime_t();
+ return static_cast<qreal>(seconds + (time.time().msec() / 1000.0));
+}
+
float limitAngleToPMPIf(float angle)
{
if (angle > -20*M_PI && angle < 20*M_PI)
diff --git a/src/QGC.h b/src/QGC.h
index 7f17bd1..15391e3 100644
--- a/src/QGC.h
+++ b/src/QGC.h
@@ -77,6 +77,8 @@ const QColor colorBlack(0, 0, 0);
quint64 groundTimeUsecs();
/** @brief Get the current ground time in milliseconds */
quint64 groundTimeMilliseconds();
+/** @brief Get the current ground time in seconds */
+qreal groundTimeSeconds();
/** @brief Returns the angle limited to -pi - pi */
float limitAngleToPMPIf(float angle);
/** @brief Returns the angle limited to -pi - pi */
diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc
index 917fa67..405dbbc 100644
--- a/src/comm/MAVLinkProtocol.cc
+++ b/src/comm/MAVLinkProtocol.cc
@@ -31,6 +31,10 @@
#include "QGCMAVLinkUASFactory.h"
#include "QGC.h"
+#ifdef QGC_PROTOBUF_ENABLED
+#include <google/protobuf/descriptor.h>
+#endif
+
/**
* The default constructor will create a new MAVLink object sending heartbeats at
@@ -219,7 +223,42 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
if (protobufManager.getMessage(protobuf_msg))
{
- emit extendedMessageReceived(link, protobuf_msg);
+ const google::protobuf::Descriptor* descriptor = protobuf_msg->GetDescriptor();
+ if (!descriptor)
+ {
+ continue;
+ }
+
+ const google::protobuf::FieldDescriptor* headerField = descriptor->FindFieldByName("header");
+ if (!headerField)
+ {
+ continue;
+ }
+
+ const google::protobuf::Descriptor* headerDescriptor = headerField->message_type();
+ if (!headerDescriptor)
+ {
+ continue;
+ }
+
+ const google::protobuf::FieldDescriptor* sourceSysIdField = headerDescriptor->FindFieldByName("source_sysid");
+ if (!sourceSysIdField)
+ {
+ continue;
+ }
+
+ const google::protobuf::Reflection* reflection = protobuf_msg->GetReflection();
+ const google::protobuf::Message& headerMsg = reflection->GetMessage(*protobuf_msg, headerField);
+ const google::protobuf::Reflection* headerReflection = headerMsg.GetReflection();
+
+ int source_sysid = headerReflection->GetInt32(headerMsg, sourceSysIdField);
+
+ UASInterface* uas = UASManager::instance()->getUASForId(source_sysid);
+
+ if (uas != NULL)
+ {
+ emit extendedMessageReceived(link, protobuf_msg);
+ }
}
}
diff --git a/src/uas/QGCUASParamManager.h b/src/uas/QGCUASParamManager.h
index db991dc..d3fedc0 100644
--- a/src/uas/QGCUASParamManager.h
+++ b/src/uas/QGCUASParamManager.h
@@ -20,8 +20,20 @@ public:
QList<QVariant> getParameterValues(int component) const {
return parameters.value(component)->values();
}
- QVariant getParameterValue(int component, const QString& parameter) const {
- return parameters.value(component)->value(parameter);
+ bool getParameterValue(int component, const QString& parameter, QVariant& value) const {
+ if (!parameters.contains(component))
+ {
+ return false;
+ }
+
+ if (!parameters.value(component)->contains(parameter))
+ {
+ return false;
+ }
+
+ value = parameters.value(component)->value(parameter);
+
+ return true;
}
virtual bool isParamMinKnown(const QString& param) = 0;
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 4e3f155..3e9e891 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -27,6 +27,10 @@
#include "LinkManager.h"
#include "SerialLink.h"
+#ifdef QGC_PROTOBUF_ENABLED
+#include <google/protobuf/descriptor.h>
+#endif
+
UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
uasId(id),
startTime(QGC::groundTimeMilliseconds()),
@@ -983,6 +987,41 @@ void UAS::receiveExtendedMessage(LinkInterface* link, std::tr1::shared_ptr<googl
addLink(link);
}
+ const google::protobuf::Descriptor* descriptor = message->GetDescriptor();
+ if (!descriptor)
+ {
+ return;
+ }
+
+ const google::protobuf::FieldDescriptor* headerField = descriptor->FindFieldByName("header");
+ if (!headerField)
+ {
+ return;
+ }
+
+ const google::protobuf::Descriptor* headerDescriptor = headerField->message_type();
+ if (!headerDescriptor)
+ {
+ return;
+ }
+
+ const google::protobuf::FieldDescriptor* sourceSysIdField = headerDescriptor->FindFieldByName("source_sysid");
+ if (!sourceSysIdField)
+ {
+ return;
+ }
+
+ const google::protobuf::Reflection* reflection = message->GetReflection();
+ const google::protobuf::Message& headerMsg = reflection->GetMessage(*message, headerField);
+ const google::protobuf::Reflection* headerReflection = headerMsg.GetReflection();
+
+ int source_sysid = headerReflection->GetInt32(headerMsg, sourceSysIdField);
+
+ if (source_sysid != uasId)
+ {
+ return;
+ }
+
if (message->GetTypeName() == pointCloud.GetTypeName())
{
pointCloud.CopyFrom(*message);
diff --git a/src/ui/map3D/ObstacleGroupNode.cc b/src/ui/map3D/ObstacleGroupNode.cc
index 72120fc..f0942a9 100644
--- a/src/ui/map3D/ObstacleGroupNode.cc
+++ b/src/ui/map3D/ObstacleGroupNode.cc
@@ -48,6 +48,15 @@ ObstacleGroupNode::init(void)
}
void
+ObstacleGroupNode::clear(void)
+{
+ if (getNumChildren() > 0)
+ {
+ removeChild(0, getNumChildren());
+ }
+}
+
+void
ObstacleGroupNode::update(MAV_FRAME frame, UASInterface *uas)
{
if (!uas || frame == MAV_FRAME_GLOBAL)
@@ -59,10 +68,7 @@ ObstacleGroupNode::update(MAV_FRAME frame, UASInterface *uas)
double robotY = uas->getLocalY();
double robotZ = uas->getLocalZ();
- if (getNumChildren() > 0)
- {
- removeChild(0, getNumChildren());
- }
+ clear();
osg::ref_ptr<osg::Geode> geode = new osg::Geode;
diff --git a/src/ui/map3D/ObstacleGroupNode.h b/src/ui/map3D/ObstacleGroupNode.h
index 0c965ce..36f1892 100644
--- a/src/ui/map3D/ObstacleGroupNode.h
+++ b/src/ui/map3D/ObstacleGroupNode.h
@@ -42,6 +42,7 @@ public:
ObstacleGroupNode();
void init(void);
+ void clear(void);
void update(MAV_FRAME frame, UASInterface* uas);
};
diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc
index dc286f9..516058d 100644
--- a/src/ui/map3D/Pixhawk3DWidget.cc
+++ b/src/ui/map3D/Pixhawk3DWidget.cc
@@ -53,6 +53,7 @@
Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent)
: Q3DWidget(parent)
, uas(NULL)
+ , kMessageTimeout(2.0)
, mode(DEFAULT_MODE)
, selectedWpIndex(-1)
, displayLocalGrid(false)
@@ -299,7 +300,7 @@ Pixhawk3DWidget::selectTargetHeading(void)
p.set(cursorWorldCoords.first, cursorWorldCoords.second);
}
- target.z() = atan2(p.y() - target.y(), p.x() - target.x());
+ target.setW(atan2(p.y() - target.y(), p.x() - target.x()));
}
void
@@ -309,6 +310,10 @@ Pixhawk3DWidget::selectTarget(void)
{
return;
}
+ if (!uas->getParamManager())
+ {
+ return;
+ }
if (frame == MAV_FRAME_GLOBAL)
{
@@ -318,7 +323,14 @@ Pixhawk3DWidget::selectTarget(void)
getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(),
altitude);
- target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0);
+ QVariant zTarget;
+ if (!uas->getParamManager()->getParameterValue(MAV_COMP_ID_PATHPLANNER, "TARGET-ALT", zTarget))
+ {
+ zTarget = -altitude;
+ }
+
+ target = QVector4D(cursorWorldCoords.first, cursorWorldCoords.second,
+ zTarget.toReal(), 0.0);
}
else if (frame == MAV_FRAME_LOCAL_NED)
{
@@ -327,7 +339,14 @@ Pixhawk3DWidget::selectTarget(void)
std::pair<double,double> cursorWorldCoords =
getGlobalCursorPosition(cachedMousePos.x(), cachedMousePos.y(), -z);
- target.set(cursorWorldCoords.first, cursorWorldCoords.second, 0.0);
+ QVariant zTarget;
+ if (!uas->getParamManager()->getParameterValue(MAV_COMP_ID_PATHPLANNER, "TARGET-ALT", zTarget))
+ {
+ zTarget = z;
+ }
+
+ target = QVector4D(cursorWorldCoords.first, cursorWorldCoords.second,
+ zTarget.toReal(), 0.0);
}
enableTarget = true;
@@ -340,8 +359,8 @@ Pixhawk3DWidget::setTarget(void)
{
selectTargetHeading();
- uas->setTargetPosition(target.x(), target.y(), 0.0,
- osg::RadiansToDegrees(target.z()));
+ uas->setTargetPosition(target.x(), target.y(), target.z(),
+ osg::RadiansToDegrees(target.w()));
}
void
@@ -761,7 +780,7 @@ Pixhawk3DWidget::display(void)
if (enableTarget)
{
- updateTarget(robotX, robotY);
+ updateTarget(robotX, robotY, robotZ);
}
#ifdef QGC_PROTOBUF_ENABLED
@@ -1493,20 +1512,21 @@ Pixhawk3DWidget::updateWaypoints(void)
}
void
-Pixhawk3DWidget::updateTarget(double robotX, double robotY)
+Pixhawk3DWidget::updateTarget(double robotX, double robotY, double robotZ)
{
osg::PositionAttitudeTransform* pat =
dynamic_cast<osg::PositionAttitudeTransform*>(targetNode.get());
- pat->setPosition(osg::Vec3d(target.y() - robotY, target.x() - robotX, 0.0));
- pat->setAttitude(osg::Quat(target.z() - M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f),
+ pat->setPosition(osg::Vec3d(target.y() - robotY,
+ target.x() - robotX,
+ -(target.z() - robotZ)));
+ pat->setAttitude(osg::Quat(target.w() - M_PI_2, osg::Vec3d(1.0f, 0.0f, 0.0f),
M_PI_2, osg::Vec3d(0.0f, 1.0f, 0.0f),
0.0, osg::Vec3d(0.0f, 0.0f, 1.0f)));
osg::Geode* geode = dynamic_cast<osg::Geode*>(pat->getChild(0));
osg::ShapeDrawable* sd = dynamic_cast<osg::ShapeDrawable*>(geode->getDrawable(0));
-
sd->setColor(osg::Vec4f(1.0f, 0.8f, 0.0f, 1.0f));
}
@@ -1607,7 +1627,14 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ)
void
Pixhawk3DWidget::updateObstacles(void)
{
- obstacleGroupNode->update(frame, uas);
+ if (QGC::groundTimeSeconds() - uas->getObstacleList().header().timestamp() < kMessageTimeout)
+ {
+ obstacleGroupNode->update(frame, uas);
+ }
+ else
+ {
+ obstacleGroupNode->clear();
+ }
}
void
@@ -1628,51 +1655,54 @@ Pixhawk3DWidget::updatePath(double robotX, double robotY, double robotZ)
osg::ref_ptr<osg::Vec3Array> vertices(new osg::Vec3Array);
- // find path length
- float length = 0.0f;
- for (int i = 0; i < path.waypoints_size() - 1; ++i)
+ if (QGC::groundTimeSeconds() - path.header().timestamp() < kMessageTimeout)
{
- const px::Waypoint& wp0 = path.waypoints(i);
- const px::Waypoint& wp1 = path.waypoints(i+1);
+ // find path length
+ float length = 0.0f;
+ for (int i = 0; i < path.waypoints_size() - 1; ++i)
+ {
+ const px::Waypoint& wp0 = path.waypoints(i);
+ const px::Waypoint& wp1 = path.waypoints(i+1);
- length += qgc::hypot3f(wp0.x() - wp1.x(),
- wp0.y() - wp1.y(),
- wp0.z() - wp1.z());
- }
+ length += qgc::hypot3f(wp0.x() - wp1.x(),
+ wp0.y() - wp1.y(),
+ wp0.z() - wp1.z());
+ }
- // build path
- if (path.waypoints_size() > 0)
- {
- const px::Waypoint& wp0 = path.waypoints(0);
+ // build path
+ if (path.waypoints_size() > 0)
+ {
+ const px::Waypoint& wp0 = path.waypoints(0);
- vertices->push_back(osg::Vec3d(wp0.y() - robotY,
- wp0.x() - robotX,
- -(wp0.z() - robotZ)));
+ vertices->push_back(osg::Vec3d(wp0.y() - robotY,
+ wp0.x() - robotX,
+ -(wp0.z() - robotZ)));
- float r, g, b;
- qgc::colormap("autumn", 0, r, g, b);
- colorArray->push_back(osg::Vec4d(r, g, b, 1.0f));
- }
+ float r, g, b;
+ qgc::colormap("autumn", 0, r, g, b);
+ colorArray->push_back(osg::Vec4d(r, g, b, 1.0f));
+ }
- float lengthCurrent = 0.0f;
- for (int i = 0; i < path.waypoints_size() - 1; ++i)
- {
- const px::Waypoint& wp0 = path.waypoints(i);
- const px::Waypoint& wp1 = path.waypoints(i+1);
+ float lengthCurrent = 0.0f;
+ for (int i = 0; i < path.waypoints_size() - 1; ++i)
+ {
+ const px::Waypoint& wp0 = path.waypoints(i);
+ const px::Waypoint& wp1 = path.waypoints(i+1);
- lengthCurrent += qgc::hypot3f(wp0.x() - wp1.x(),
- wp0.y() - wp1.y(),
- wp0.z() - wp1.z());
+ lengthCurrent += qgc::hypot3f(wp0.x() - wp1.x(),
+ wp0.y() - wp1.y(),
+ wp0.z() - wp1.z());
- vertices->push_back(osg::Vec3d(wp1.y() - robotY,
- wp1.x() - robotX,
- -(wp1.z() - robotZ)));
+ vertices->push_back(osg::Vec3d(wp1.y() - robotY,
+ wp1.x() - robotX,
+ -(wp1.z() - robotZ)));
- int colorIdx = lengthCurrent / length * 127.0f;
+ int colorIdx = lengthCurrent / length * 127.0f;
- float r, g, b;
- qgc::colormap("autumn", colorIdx, r, g, b);
- colorArray->push_back(osg::Vec4f(r, g, b, 1.0f));
+ float r, g, b;
+ qgc::colormap("autumn", colorIdx, r, g, b);
+ colorArray->push_back(osg::Vec4f(r, g, b, 1.0f));
+ }
}
geometry->setVertexArray(vertices);
diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h
index f67cf1a..8adfb1f 100644
--- a/src/ui/map3D/Pixhawk3DWidget.h
+++ b/src/ui/map3D/Pixhawk3DWidget.h
@@ -124,7 +124,7 @@ private:
void updateImagery(double originX, double originY, double originZ,
const QString& zone);
void updateWaypoints(void);
- void updateTarget(double robotX, double robotY);
+ void updateTarget(double robotX, double robotY, double robotZ);
#ifdef QGC_PROTOBUF_ENABLED
void updateRGBD(double robotX, double robotY, double robotZ);
void updateObstacles(void);
@@ -136,6 +136,8 @@ private:
void showInsertWaypointMenu(const QPoint& cursorPos);
void showEditWaypointMenu(const QPoint& cursorPos);
+ const qreal kMessageTimeout; // message timeout in seconds
+
enum Mode {
DEFAULT_MODE,
MOVE_WAYPOINT_POSITION_MODE,
@@ -184,7 +186,7 @@ private:
QVector< osg::ref_ptr<osg::Node> > vehicleModels;
MAV_FRAME frame;
- osg::Vec3d target;
+ QVector4D target;
QPoint cachedMousePos;
double lastRobotX, lastRobotY, lastRobotZ;
};