Code changes to reflect new header information (sysid, compid, timestamp) in...

Code changes to reflect new header information (sysid, compid, timestamp) in protobuf messages. Added protobuf message timeout; old messages will not be visualized. Fixed getParameterValue in ParamManager; the method can segfault if the component id or parameter id is not valid.

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)

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 */

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
@@ -218,10 +222,45 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
                     std::tr1::shared_ptr<google::protobuf::Message> protobuf_msg;
 
                     if (protobufManager.getMessage(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);
                         }
                     }
+                }
 
                 position += extended_message.extended_payload_len;
 

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;

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);

src/ui/map3D/ObstacleGroupNode.cc

@@ -47,6 +47,15 @@ ObstacleGroupNode::init(void)
 
 }
 
+void
+ObstacleGroupNode::clear(void)
+{
+    if (getNumChildren() > 0)
+    {
+        removeChild(0, getNumChildren());
+    }
+}
+
 void
 ObstacleGroupNode::update(MAV_FRAME frame, UASInterface *uas)
 {
@@ -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;
 

src/ui/map3D/ObstacleGroupNode.h

@@ -42,6 +42,7 @@ public:
     ObstacleGroupNode();
 
     void init(void);
+    void clear(void);
     void update(MAV_FRAME frame, UASInterface* uas);
 };
 

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)
 {
+    if (QGC::groundTimeSeconds() - uas->getObstacleList().header().timestamp() < kMessageTimeout)
+    {
         obstacleGroupNode->update(frame, uas);
+    }
+    else
+    {
+        obstacleGroupNode->clear();
+    }
 }
 
 void
@@ -1628,6 +1655,8 @@ Pixhawk3DWidget::updatePath(double robotX, double robotY, double robotZ)
 
     osg::ref_ptr<osg::Vec3Array> vertices(new osg::Vec3Array);
 
+    if (QGC::groundTimeSeconds() - path.header().timestamp() < kMessageTimeout)
+    {
         // find path length
         float length = 0.0f;
         for (int i = 0; i < path.waypoints_size() - 1; ++i)
@@ -1674,6 +1703,7 @@ Pixhawk3DWidget::updatePath(double robotX, double robotY, double robotZ)
             qgc::colormap("autumn", colorIdx, r, g, b);
             colorArray->push_back(osg::Vec4f(r, g, b, 1.0f));
         }
+    }
 
     geometry->setVertexArray(vertices);
     drawArrays->setFirst(0);

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;
 };