diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index 362de7fc5f8c75ddc993bc10d61d4f802fc50b81..3954efb371a18eaa0c76e206ca2149adbb05a895 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -108,6 +108,19 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "vis. z", "m", pos.z, time); } break; + case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE: { + mavlink_global_vision_position_estimate_t pos; + mavlink_msg_global_vision_position_estimate_decode(&message, &pos); + quint64 time = getUnixTime(pos.usec); + //emit valueChanged(uasId, "vis. time", pos.usec, time); + emit valueChanged(uasId, "glob. vis. roll", "rad", pos.roll, time); + emit valueChanged(uasId, "glob. vis. pitch", "rad", pos.pitch, time); + emit valueChanged(uasId, "glob. vis. yaw", "rad", pos.yaw, time); + emit valueChanged(uasId, "glob. vis. x", "m", pos.x, time); + emit valueChanged(uasId, "glob. vis. y", "m", pos.y, time); + emit valueChanged(uasId, "glob. vis. z", "m", pos.z, time); + } + break; case MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE: { mavlink_vicon_position_estimate_t pos; mavlink_msg_vicon_position_estimate_decode(&message, &pos);