diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index 83f834706d9293c692196c9f4f670f3ae6581317..a229b59f29a8e5331a1668669c4ea198bbea06e4 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -641,7 +641,7 @@ void QGCXPlaneLink::readBytes() float eph = 0.3; float epv = 0.6; float vel = sqrt(vx*vx + vy*vy + vz*vz); - float cog = yaw; + float cog = ((yaw + M_PI) / M_PI) * 180.0f; int satellites = 8; emit sensorHilGpsChanged(QGC::groundTimeUsecs(), lat, lon, alt, gps_fix_type, eph, epv, vel, cog, satellites); diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 64623f4cf284e4b8c414fee3a534b438cf57c2d2..cf0575dc0a05a0da8499bccf97ab61689966c4f5 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -111,7 +111,8 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), lastVoltageWarning(0), lastNonNullTime(0), onboardTimeOffsetInvalidCount(0), - hilEnabled(false) + hilEnabled(false), + lastSendTimeGPS(0) { for (unsigned int i = 0; i<255;++i) @@ -2785,11 +2786,16 @@ void UAS::sendHilSensors(quint64 time_us, float xacc, float yacc, float zacc, fl void UAS::sendHilGps(quint64 time_us, double lat, double lon, double alt, int fix_type, float eph, float epv, float vel, float cog, int satellites) { + // Only send at 10 Hz max rate + if (QGC::groundTimeMilliseconds() - lastSendTimeGPS < 100) + return; + if (this->mode & MAV_MODE_FLAG_HIL_ENABLED) { mavlink_message_t msg; mavlink_msg_gps_raw_int_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, time_us, fix_type, lat*1e7, lon*1e7, alt*1e3, eph*1e2, epv*1e2, vel*1e2, cog*1e2, satellites); + lastSendTimeGPS = QGC::groundTimeMilliseconds(); sendMessage(msg); } else diff --git a/src/uas/UAS.h b/src/uas/UAS.h index e4dfd783e4a61f8d5fd83d845163b6d473b2d86a..4bc7948928acdf731081683ab010db31d8c37a28 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -704,6 +704,7 @@ protected: unsigned int onboardTimeOffsetInvalidCount; ///< Count when the offboard time offset estimation seemed wrong bool hilEnabled; ///< Set to true if HIL mode is enabled from GCS (UAS might be in HIL even if this flag is not set, this defines the GCS HIL setting) bool sensorHil; ///< True if sensor HIL is enabled + quint64 lastSendTimeGPS; ///< Last HIL GPS message sent protected slots: /** @brief Write settings to disk */