From 9082fac0d7e7880f9d63977eaab56e1734f8bed9 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Mon, 28 Sep 2015 09:51:06 -0700
Subject: [PATCH] Show home position on flight map

---
 src/FlightDisplay/FlightDisplayView.qml |  8 +++++++
 src/Vehicle/Vehicle.cc                  | 29 +++++++++++++++++++++++++
 src/Vehicle/Vehicle.h                   | 11 ++++++++++
 3 files changed, 48 insertions(+)

diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml
index 70bc9d136..b5f4b4c7c 100644
--- a/src/FlightDisplay/FlightDisplayView.qml
+++ b/src/FlightDisplay/FlightDisplayView.qml
@@ -26,6 +26,7 @@ import QtQuick.Controls         1.3
 import QtQuick.Controls.Styles  1.2
 import QtQuick.Dialogs          1.2
 import QtLocation               5.3
+import QtPositioning            5.2
 
 import QGroundControl.FlightMap     1.0
 import QGroundControl.ScreenTools   1.0
@@ -92,6 +93,13 @@ Item {
         longitude:      parent._longitude
         visible:        _showMap
 
+        // Home position
+        MissionItemIndicator {
+            label:          "H"
+            coordinate:     (_activeVehicle && _activeVehicle.homePositionAvailable) ? _activeVehicle.homePosition : QtPositioning.coordinate(0, 0)
+            visible:        _activeVehicle ? _activeVehicle.homePositionAvailable : false
+        }
+
         // Add the vehicles to the map
         MapItemView {
             model: multiVehicleManager.vehicles
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 7e9737125..6bcc7a9b2 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -51,6 +51,7 @@ Vehicle::Vehicle(LinkInterface* link, int vehicleId, MAV_AUTOPILOT firmwareType)
     , _joystickMode(JoystickModeRC)
     , _joystickEnabled(false)
     , _uas(NULL)
+    , _homePositionAvailable(false)
     , _mav(NULL)
     , _currentMessageCount(0)
     , _messageCount(0)
@@ -203,6 +204,20 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     // Give the plugin a change to adjust the message contents
     _firmwarePlugin->adjustMavlinkMessage(&message);
     
+    if (message.msgid == MAVLINK_MSG_ID_HOME_POSITION) {
+        mavlink_home_position_t homePos;
+        
+        mavlink_msg_home_position_decode(&message, &homePos);
+        
+        _homePosition.setLatitude(homePos.latitude / 10000000.0);
+        _homePosition.setLongitude(homePos.longitude / 10000000.0);
+        _homePosition.setAltitude(homePos.altitude / 1000.0);
+        _homePositionAvailable = true;
+        
+        emit homePositionAvailableChanged(true);
+        emit homePositionChanged(_homePosition);
+    }
+    
     emit mavlinkMessageReceived(message);
     
     _uas->receiveMessage(message);
@@ -945,3 +960,17 @@ QmlObjectListModel* Vehicle::missionItemsModel(void)
         return &_missionItems;
     }
 }
+
+bool Vehicle::homePositionAvailable(void)
+{
+    return _homePositionAvailable;
+}
+
+QGeoCoordinate Vehicle::homePosition(void)
+{
+    if (!_homePositionAvailable) {
+        qWarning() << "Call to homePosition while _homePositionAvailable == false";
+    }
+    
+    return _homePosition;
+}
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index 1c30ed9bc..cffb42170 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -59,6 +59,9 @@ public:
     Q_PROPERTY(QGeoCoordinate   coordinate      MEMBER _geoCoordinate   NOTIFY coordinateChanged)
     Q_PROPERTY(MissionManager*  missionManager  MEMBER _missionManager  CONSTANT)
     
+    Q_PROPERTY(bool             homePositionAvailable   READ homePositionAvailable  NOTIFY homePositionAvailableChanged)
+    Q_PROPERTY(QGeoCoordinate   homePosition            READ homePosition           NOTIFY homePositionChanged)
+    
     Q_INVOKABLE QString     getMavIconColor();
     
     //-- System Messages
@@ -154,6 +157,9 @@ public:
     
     MissionManager* missionManager(void) { return _missionManager; }
     
+    bool homePositionAvailable(void);
+    QGeoCoordinate homePosition(void);
+    
     typedef enum {
         MessageNone,
         MessageNormal,
@@ -221,6 +227,8 @@ signals:
     void joystickEnabledChanged(bool enabled);
     void activeChanged(bool active);
     void mavlinkMessageReceived(const mavlink_message_t& message);
+    void homePositionAvailableChanged(bool homePositionAvailable);
+    void homePositionChanged(const QGeoCoordinate& homePosition);
     
     /// Used internally to move sendMessage call to main thread
     void _sendMessageOnThread(mavlink_message_t message);
@@ -318,6 +326,9 @@ private:
     
     QGeoCoordinate  _geoCoordinate;
     
+    bool            _homePositionAvailable;
+    QGeoCoordinate  _homePosition;
+    
     UASInterface*   _mav;
     int             _currentMessageCount;
     int             _messageCount;
-- 
2.22.0