diff --git a/src/comm/MAVLinkSimulationMAV.cc b/src/comm/MAVLinkSimulationMAV.cc index 8f8e8ee..9576399 100644 --- a/src/comm/MAVLinkSimulationMAV.cc +++ b/src/comm/MAVLinkSimulationMAV.cc @@ -167,7 +167,7 @@ void MAVLinkSimulationMAV::mainloop() // Name of the value, maximum 10 characters // see full message specs at: // http://pixhawk.ethz.ch/wiki/mavlink/ - strcpy(val.name, "FLOAT"); + strcpy((char *)val.name, "FLOAT"); // Value, in this case 0.5 val.value = 0.5f; @@ -195,7 +195,7 @@ void MAVLinkSimulationMAV::mainloop() // Name of the value, maximum 10 characters // see full message specs at: // http://pixhawk.ethz.ch/wiki/mavlink/ - strcpy(valint.name, "INTEGER"); + strcpy((char *)valint.name, "INTEGER"); // Value, in this case 18000 valint.value = 18000; diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 1baec1b..4e8eef7 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -176,13 +176,13 @@ void UAS::receiveMessageNamedValue(const mavlink_message_t& message) { mavlink_named_value_float_t val; mavlink_msg_named_value_float_decode(&message, &val); - emit valueChanged(this->getUASID(), QString(val.name), tr("raw"), val.value, getUnixTime(0)); + emit valueChanged(this->getUASID(), QString((char *)val.name), tr("raw"), val.value, getUnixTime(0)); } else if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT) { mavlink_named_value_int_t val; mavlink_msg_named_value_int_decode(&message, &val); - emit valueChanged(this->getUASID(), QString(val.name), tr("raw"), (float)val.value, getUnixTime(0)); + emit valueChanged(this->getUASID(), QString((char *)val.name), tr("raw"), (float)val.value, getUnixTime(0)); } } @@ -476,6 +476,36 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) forwardMessage(message); } break; + case MAVLINK_MSG_ID_GLOBAL_POSITION: + { + mavlink_global_position_t pos; + mavlink_msg_global_position_decode(&message, &pos); + quint64 time = QGC::groundTimeUsecs()/1000; + latitude = pos.lat; + longitude = pos.lon; + altitude = pos.alt; + speedX = pos.vx; + speedY = pos.vy; + speedZ = pos.vz; + emit valueChanged(uasId, "latitude", "deg", latitude, time); + emit valueChanged(uasId, "longitude", "deg", longitude, time); + emit valueChanged(uasId, "altitude", "m", altitude, time); + emit valueChanged(uasId, "gps x speed", "m/s", speedX, time); + emit valueChanged(uasId, "gps y speed", "m/s", speedY, time); + emit valueChanged(uasId, "gps z speed", "m/s", speedZ, time); + emit globalPositionChanged(this, longitude, latitude, altitude, time); + emit speedChanged(this, speedX, speedY, speedZ, time); + // Set internal state + if (!positionLock) + { + // If position was not locked before, notify positive + GAudioOutput::instance()->notifyPositive(); + } + positionLock = true; + //TODO fix this hack for forwarding of global position for patch antenna tracking + forwardMessage(message); + } + break; case MAVLINK_MSG_ID_GPS_RAW: //std::cerr << std::endl; //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;