diff --git a/src/QGC.cc b/src/QGC.cc index c30209f83b0ceb8ea992d4d465dd9f385838f53c..50eb1c01754cac4f0018f8438ed6e68eb58901da 100644 --- a/src/QGC.cc +++ b/src/QGC.cc @@ -47,6 +47,15 @@ quint64 groundTimeMilliseconds() return static_cast(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(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 7f17bd143cb4ef2a6641e6120d56f3aec214252e..15391e315dddd7db2c76b04fc3c8decbb68853cb 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 917fa67109c49d60f52bdb6b06c19df51b8a9004..405dbbc0cd3d865377c0e1df593f62378d284e09 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 +#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 db991dcb7c6e32941643df0898e996921489095e..d3fedc04f5c171a3b0caa7cd9a50c57d8d5c2ef3 100644 --- a/src/uas/QGCUASParamManager.h +++ b/src/uas/QGCUASParamManager.h @@ -20,8 +20,20 @@ public: QList 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 4e3f155b4284b44e77f351482c76e7e4248bed98..3e9e8913e9f322135f7952c7dd55c0d80cc3cc55 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 +#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_ptrGetDescriptor(); + 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 72120fcea2d58a73c79272beeb396bdd1ef6e7d0..f0942a97d46e1dde61f4fef5e8cf78175c007e89 100644 --- a/src/ui/map3D/ObstacleGroupNode.cc +++ b/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 geode = new osg::Geode; diff --git a/src/ui/map3D/ObstacleGroupNode.h b/src/ui/map3D/ObstacleGroupNode.h index 0c965cef0d94ee150b1d14ec3715dbae3255ebef..36f1892a614d07974cdb99d3a364caf8f57c92b8 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 dc286f916b52de05a99ea4a0775e458359ed13c7..516058d330fa6b964bf695a498481dfcd1f1896f 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 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(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(pat->getChild(0)); osg::ShapeDrawable* sd = dynamic_cast(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 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 f67cf1a2cd472643f291f8ee6fb79d0d73f7d24d..8adfb1fa59b1d506aa109f1950dc2c0529683619 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 > vehicleModels; MAV_FRAME frame; - osg::Vec3d target; + QVector4D target; QPoint cachedMousePos; double lastRobotX, lastRobotY, lastRobotZ; };