From 4d396823b62f055316c282064f434241f34493b4 Mon Sep 17 00:00:00 2001
From: DonLakeFlyer <dongagne@outlook.com>
Date: Mon, 30 Sep 2019 14:52:20 -0700
Subject: [PATCH] Fix correct usage of heading value

---
 src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 9 ++++++---
 src/FollowMe/FollowMe.cc                    | 1 +
 src/FollowMe/FollowMe.h                     | 3 ++-
 3 files changed, 9 insertions(+), 4 deletions(-)

diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
index e8fd34d17..91ea25f9a 100644
--- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc
@@ -14,6 +14,7 @@
 #include "APMFlightModesComponentController.h"
 #include "APMAirframeComponentController.h"
 #include "APMSensorsComponentController.h"
+#include "APMFollowComponentController.h"
 #include "MissionManager.h"
 #include "ParameterManager.h"
 #include "QGCFileDownload.h"
@@ -158,6 +159,7 @@ APMFirmwarePlugin::APMFirmwarePlugin(void)
     qmlRegisterType<APMFlightModesComponentController>  ("QGroundControl.Controllers", 1, 0, "APMFlightModesComponentController");
     qmlRegisterType<APMAirframeComponentController>     ("QGroundControl.Controllers", 1, 0, "APMAirframeComponentController");
     qmlRegisterType<APMSensorsComponentController>      ("QGroundControl.Controllers", 1, 0, "APMSensorsComponentController");
+    qmlRegisterType<APMFollowComponentController>       ("QGroundControl.Controllers", 1, 0, "APMFollowComponentController");
 }
 
 AutoPilotPlugin* APMFirmwarePlugin::autopilotPlugin(Vehicle* vehicle)
@@ -396,7 +398,8 @@ bool APMFirmwarePlugin::_handleIncomingStatusText(Vehicle* vehicle, mavlink_mess
                 }
 
                 if (supportedMajorNumber != -1) {
-                    if (firmwareVersion.majorNumber() < supportedMajorNumber || firmwareVersion.minorNumber() < supportedMinorNumber) {
+                    if (firmwareVersion.majorNumber() < supportedMajorNumber ||
+                            (firmwareVersion.majorNumber() == supportedMajorNumber && firmwareVersion.minorNumber() < supportedMinorNumber)) {
                         qgcApp()->showMessage(tr("QGroundControl fully supports Version %1.%2 and above. You are using a version prior to that. This combination is untested, you may run into unpredictable results.").arg(supportedMajorNumber).arg(supportedMinorNumber));
                     }
                 }
@@ -1101,7 +1104,7 @@ void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMoti
         return;
     }
 
-    if (!(estimationCapabilities & (FollowMe::POS | FollowMe::VEL))) {
+    if (!(estimationCapabilities & (FollowMe::POS | FollowMe::VEL | FollowMe::HEADING))) {
         static bool sentOnce = false;
         if (!sentOnce) {
             sentOnce = true;
@@ -1124,7 +1127,7 @@ void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMoti
     globalPositionInt.vx =              static_cast<int16_t>(motionReport.vxMetersPerSec * 100);                                        // cm/sec
     globalPositionInt.vy =              static_cast<int16_t>(motionReport.vyMetersPerSec * 100);                                        // cm/sec
     globalPositionInt.vy =              static_cast<int16_t>(motionReport.vzMetersPerSec * 100);                                        // cm/sec
-    globalPositionInt.hdg =             UINT16_MAX;
+    globalPositionInt.hdg =             static_cast<uint16_t>(motionReport.headingDegrees * 100.0);                                     // centi-degrees
 
     mavlink_message_t message;
     mavlink_msg_global_position_int_encode_chan(static_cast<uint8_t>(mavlinkProtocol->getSystemId()),
diff --git a/src/FollowMe/FollowMe.cc b/src/FollowMe/FollowMe.cc
index 2eb0f0ed9..3d8945f8b 100644
--- a/src/FollowMe/FollowMe.cc
+++ b/src/FollowMe/FollowMe.cc
@@ -93,6 +93,7 @@ void FollowMe::_sendGCSMotionReport()
     estimatation_capabilities |=    (1 << POS);
 
     if (geoPositionInfo.hasAttribute(QGeoPositionInfo::Direction) == true) {
+        estimatation_capabilities |= (1 << HEADING);
         motionReport.headingDegrees = geoPositionInfo.attribute(QGeoPositionInfo::Direction);
     }
 
diff --git a/src/FollowMe/FollowMe.h b/src/FollowMe/FollowMe.h
index e17bcab1f..6585a2497 100644
--- a/src/FollowMe/FollowMe.h
+++ b/src/FollowMe/FollowMe.h
@@ -47,7 +47,8 @@ public:
         POS = 0,
         VEL = 1,
         ACCEL = 2,
-        ATT_RATES = 3
+        ATT_RATES = 3,
+        HEADING = 4
     };
 
     void setToolbox(QGCToolbox* toolbox) override;
-- 
2.22.0