diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 67e7ca455abf9cd71c265d34f127147ea3cb9ae2..a2b4a8374980f67cbf39bb1da7cb2e326e4aff2a 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -473,37 +473,40 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) break; case MAVLINK_MSG_ID_ATTITUDE: { - if (wrongComponent) break; - mavlink_attitude_t attitude; mavlink_msg_attitude_decode(&message, &attitude); quint64 time = getUnixReferenceTime(attitude.time_boot_ms); - lastAttitude = time; - roll = QGC::limitAngleToPMPIf(attitude.roll); - pitch = QGC::limitAngleToPMPIf(attitude.pitch); - yaw = QGC::limitAngleToPMPIf(attitude.yaw); - - // // Emit in angles - - // // Convert yaw angle to compass value - // // in 0 - 360 deg range - // float compass = (yaw/M_PI)*180.0+360.0f; - // if (compass > -10000 && compass < 10000) - // { - // while (compass > 360.0f) { - // compass -= 360.0f; - // } - // } - // else - // { - // // Set to 0, since it is an invalid value - // compass = 0.0f; - // } - attitudeKnown = true; - emit attitudeChanged(this, roll, pitch, yaw, time); - emit attitudeChanged(this, message.compid, roll, pitch, yaw, time); - emit attitudeSpeedChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); + emit attitudeChanged(this, message.compid, QGC::limitAngleToPMPIf(attitude.roll), QGC::limitAngleToPMPIf(attitude.pitch), QGC::limitAngleToPMPIf(attitude.yaw), time); + + if (!wrongComponent) + { + lastAttitude = time; + roll = QGC::limitAngleToPMPIf(attitude.roll); + pitch = QGC::limitAngleToPMPIf(attitude.pitch); + yaw = QGC::limitAngleToPMPIf(attitude.yaw); + + // // Emit in angles + + // // Convert yaw angle to compass value + // // in 0 - 360 deg range + // float compass = (yaw/M_PI)*180.0+360.0f; + // if (compass > -10000 && compass < 10000) + // { + // while (compass > 360.0f) { + // compass -= 360.0f; + // } + // } + // else + // { + // // Set to 0, since it is an invalid value + // compass = 0.0f; + // } + + attitudeKnown = true; + emit attitudeChanged(this, roll, pitch, yaw, time); + emit attitudeSpeedChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time); + } } break; case MAVLINK_MSG_ID_HIL_CONTROLS: diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index 714ab9e1a8fab5444e71ee5a9c1d576db34d121f..51a374902988a1da877637cbc8a81245e2240327 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -129,6 +129,8 @@ Pixhawk3DWidget::systemCreated(UASInterface *uas) this, SLOT(localPositionChanged(UASInterface*,int,double,double,double,quint64))); connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(localPositionChanged(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(attitudeChanged(UASInterface*,int,double,double,double,quint64)), + this, SLOT(attitudeChanged(UASInterface*,int,double,double,double,quint64))); connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(attitudeChanged(UASInterface*,double,double,double,quint64))); connect(uas, SIGNAL(userPositionSetPointsChanged(int,float,float,float,float)), @@ -168,6 +170,50 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, int component, systemData.trailNode()->addDrawable(createTrail(color)); systemData.trailNode()->addDrawable(createLink(uas->getColor())); + + double radius = 0.5; + + osg::ref_ptr group = new osg::Group; + + // cone indicates orientation + osg::ref_ptr sd = new osg::ShapeDrawable; + double coneRadius = radius / 2.0; + osg::ref_ptr cone = + new osg::Cone(osg::Vec3d(0.0, 0.0, 0.0), + coneRadius, radius * 2.0); + + sd->setShape(cone); + sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); + sd->setColor(color); + + osg::ref_ptr geode = new osg::Geode; + geode->addDrawable(sd); + + osg::ref_ptr pat = + new osg::PositionAttitudeTransform; + pat->addChild(geode); + pat->setAttitude(osg::Quat(- 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))); + group->addChild(pat); + + // cylinder indicates position + sd = new osg::ShapeDrawable; + osg::ref_ptr cylinder = + new osg::Cylinder(osg::Vec3d(0.0, 0.0, 0.0), + radius, 0); + + sd->setShape(cylinder); + sd->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); + sd->setColor(color); + + geode = new osg::Geode; + geode->addDrawable(sd); + group->addChild(geode); + + pat = new osg::PositionAttitudeTransform; + pat->addChild(group); + systemData.orientationNode()->addChild(pat); } QVector& trail = systemData.trailMap()[component]; @@ -219,6 +265,36 @@ Pixhawk3DWidget::localPositionChanged(UASInterface* uas, m3DWidget->systemGroup(systemId)->position()->setPosition(osg::Vec3d(y, x, -z)); } +void +Pixhawk3DWidget::attitudeChanged(UASInterface* uas, int component, + double roll, double pitch, double yaw, + quint64 time) +{ + int systemId = uas->getUASID(); + + if (!mSystemContainerMap.contains(systemId)) + { + return; + } + + SystemContainer& systemData = mSystemContainerMap[systemId]; + + // update trail data + if (!systemData.trailMap().contains(component)) + { + return; + } + + int idx = systemData.trailIndexMap().value(component); + + osg::PositionAttitudeTransform* pat = + dynamic_cast(systemData.orientationNode()->getChild(idx)); + + pat->setAttitude(osg::Quat(-yaw, osg::Vec3d(0.0f, 0.0f, 1.0f), + 0.0, osg::Vec3d(1.0f, 0.0f, 0.0f), + 0.0, osg::Vec3d(0.0f, 1.0f, 0.0f))); +} + void Pixhawk3DWidget::attitudeChanged(UASInterface* uas, double roll, double pitch, double yaw, @@ -725,6 +801,8 @@ Pixhawk3DWidget::update(void) osg::ref_ptr& rollingMap = systemNode->rollingMap(); rollingMap->setChildValue(systemData.localGridNode(), systemViewParams->displayLocalGrid()); + rollingMap->setChildValue(systemData.orientationNode(), + systemViewParams->displayTrails()); rollingMap->setChildValue(systemData.pointCloudNode(), systemViewParams->displayPointCloud()); rollingMap->setChildValue(systemData.targetNode(), @@ -826,7 +904,7 @@ Pixhawk3DWidget::update(void) } if (systemViewParams->displayTrails()) { - updateTrails(x, y, z, systemData.trailNode(), + updateTrails(x, y, z, systemData.trailNode(), systemData.orientationNode(), systemData.trailMap(), systemData.trailIndexMap()); } else @@ -1094,6 +1172,10 @@ Pixhawk3DWidget::initializeSystem(int systemId, const QColor& systemColor) systemData.localGridNode() = createLocalGrid(); systemNode->rollingMap()->addChild(systemData.localGridNode(), false); + // generate orientation model + systemData.orientationNode() = new osg::Group; + systemNode->rollingMap()->addChild(systemData.orientationNode(), false); + // generate point cloud model systemData.pointCloudNode() = createPointCloud(); systemNode->rollingMap()->addChild(systemData.pointCloudNode(), false); @@ -1668,6 +1750,7 @@ Pixhawk3DWidget::updateHUD(UASInterface* uas, MAV_FRAME frame) void Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ, osg::ref_ptr& trailNode, + osg::ref_ptr& orientationNode, QMap >& trailMap, QMap& trailIndexMap) { @@ -1729,6 +1812,15 @@ Pixhawk3DWidget::updateTrails(double robotX, double robotY, double robotZ, drawArrays->setFirst(0); drawArrays->setCount(vertices->size()); geometry->dirtyBound(); + + if (!trail.empty()) + { + osg::PositionAttitudeTransform* pat = + dynamic_cast(orientationNode->getChild(it.value())); + pat->setPosition(osg::Vec3(trail.back().y() - robotY, + trail.back().x() - robotX, + -(trail.back().z() - robotZ))); + } } } diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index 8b3eff000ba52ddfb9f56c01a81f8039bc84c01e..b7713a13025e939af92b564bc29970e6d602cff9 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -58,6 +58,7 @@ public slots: void systemCreated(UASInterface* uas); void localPositionChanged(UASInterface* uas, int component, double x, double y, double z, quint64 time); void localPositionChanged(UASInterface* uas, double x, double y, double z, quint64 time); + void attitudeChanged(UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 time); void attitudeChanged(UASInterface* uas, double roll, double pitch, double yaw, quint64 time); void setpointChanged(int uasId, float x, float y, float z, float yaw); @@ -142,6 +143,7 @@ private: osg::ref_ptr& targetNode); void updateTrails(double robotX, double robotY, double robotZ, osg::ref_ptr& trailNode, + osg::ref_ptr& orientationNode, QMap >& trailMap, QMap& trailIndexMap); void updateWaypoints(UASInterface* uas, MAV_FRAME frame, diff --git a/src/ui/map3D/SystemContainer.cc b/src/ui/map3D/SystemContainer.cc index 2c3f842ab4d0f72ac980a0864ece2d554785a6cf..9a9025a3dfb6916597765232ecf5676f8dc01696 100644 --- a/src/ui/map3D/SystemContainer.cc +++ b/src/ui/map3D/SystemContainer.cc @@ -49,6 +49,12 @@ SystemContainer::modelNode(void) return mModelNode; } +osg::ref_ptr& +SystemContainer::orientationNode(void) +{ + return mOrientationNode; +} + osg::ref_ptr& SystemContainer::pointCloudNode(void) { diff --git a/src/ui/map3D/SystemContainer.h b/src/ui/map3D/SystemContainer.h index f6224e84d77155a9c5f5b986145fbaef184b2106..25ec67933804da96c802bc1f179ba24566617fb5 100644 --- a/src/ui/map3D/SystemContainer.h +++ b/src/ui/map3D/SystemContainer.h @@ -28,6 +28,7 @@ public: osg::ref_ptr& depthImageNode(void); osg::ref_ptr& localGridNode(void); osg::ref_ptr& modelNode(void); + osg::ref_ptr& orientationNode(void); osg::ref_ptr& pointCloudNode(void); osg::ref_ptr& rgbImageNode(void); osg::ref_ptr& setpointGroupNode(void); @@ -52,6 +53,7 @@ private: osg::ref_ptr mDepthImageNode; osg::ref_ptr mLocalGridNode; osg::ref_ptr mModelNode; + osg::ref_ptr mOrientationNode; osg::ref_ptr mPointCloudNode; osg::ref_ptr mRGBImageNode; osg::ref_ptr mSetpointGroupNode;