diff --git a/src/uas/PxQuadMAV.cc b/src/uas/PxQuadMAV.cc index b2c8392b282f2c06990b77ecc8e99cc538e7f9f1..17059c30ec1a045e96c1a0a6a7eae498befb4941 100644 --- a/src/uas/PxQuadMAV.cc +++ b/src/uas/PxQuadMAV.cc @@ -19,6 +19,9 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) UAS::receiveMessage(link, message); mavlink_message_t* msg = &message; + qDebug() << "PX RECEIVED" << msg->sysid << msg->compid << msg->msgid; + + // Handle your special messages switch (msg->msgid) { diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index c8de323927cf96041882cae582d0a089750542f7..5ec3f53c324c94a7b132c7a3bdfa0f8383334918 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -117,6 +117,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) addLink(link); } + qDebug() << "UAS RECEIVED" << message.sysid << message.compid << message.msgid; + if (message.sysid == uasId) { QString uasState; @@ -368,13 +370,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) { mavlink_gps_raw_t pos; mavlink_msg_gps_raw_decode(&message, &pos); - quint64 time = getUnixTime(pos.usec); + // quint64 time = getUnixTime(pos.usec); + quint64 time = MG::TIME::getGroundTimeNow(); emit valueChanged(uasId, "lat", pos.lat, time); emit valueChanged(uasId, "lon", pos.lon, time); emit valueChanged(uasId, "alt", pos.alt, time); emit valueChanged(uasId, "g-vx", pos.vx, time); emit valueChanged(uasId, "g-vy", pos.vy, time); emit valueChanged(uasId, "g-vz", pos.vz, time); + qDebug() << "GOT GPS RAW"; + emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time); } break; case MAVLINK_MSG_ID_PARAM_VALUE: