Support live home position in Mission Editor

src/MissionEditor/MissionEditor.cc

@@ -38,6 +38,8 @@ MissionEditor::MissionEditor(QWidget *parent)
     : QGCQmlWidgetHolder(parent)
     , _missionItems(NULL)
     , _canEdit(true)
+    , _activeVehicle(NULL)
+    , _liveHomePositionAvailable(false)
 {
     // Get rid of layout default margins
     QLayout* pl = layout();
@@ -45,11 +47,16 @@ MissionEditor::MissionEditor(QWidget *parent)
         pl->setContentsMargins(0,0,0,0);
     }
     
-    Vehicle* activeVehicle = MultiVehicleManager::instance()->activeVehicle();
+    MultiVehicleManager* multiVehicleMgr = MultiVehicleManager::instance();
+    
+    connect(multiVehicleMgr, &MultiVehicleManager::activeVehicleChanged, this, &MissionEditor::_activeVehicleChanged);
+    
+    Vehicle* activeVehicle = multiVehicleMgr->activeVehicle();
     if (activeVehicle) {
         MissionManager* missionManager = activeVehicle->missionManager();
         connect(missionManager, &MissionManager::newMissionItemsAvailable, this, &MissionEditor::_newMissionItemsAvailable);
         _newMissionItemsAvailable();
+        _activeVehicleChanged(activeVehicle);
     } else {
         _missionItems = new QmlObjectListModel(this);
         _initAllMissionItems();
@@ -389,3 +396,34 @@ void MissionEditor::_itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command
     _recalcChildItems();
     _recalcWaypointLines();
 }
+
+void MissionEditor::_activeVehicleChanged(Vehicle* activeVehicle)
+{
+    if (_activeVehicle) {
+        disconnect(_activeVehicle, &Vehicle::homePositionAvailableChanged,  this, &MissionEditor::_activeVehicleHomePositionAvailableChanged);
+        disconnect(_activeVehicle, &Vehicle::homePositionChanged,           this, &MissionEditor::_activeVehicleHomePositionChanged);
+        _activeVehicle = NULL;
+        _activeVehicleHomePositionAvailableChanged(false);
+    }
+    
+    _activeVehicle = activeVehicle;
+    
+    if (_activeVehicle) {
+        connect(_activeVehicle, &Vehicle::homePositionAvailableChanged, this, &MissionEditor::_activeVehicleHomePositionAvailableChanged);
+        connect(_activeVehicle, &Vehicle::homePositionChanged,          this, &MissionEditor::_activeVehicleHomePositionChanged);
+        _activeVehicleHomePositionChanged(_activeVehicle->homePosition());
+        _activeVehicleHomePositionAvailableChanged(_activeVehicle->homePositionAvailable());
+    }
+}
+
+void MissionEditor::_activeVehicleHomePositionAvailableChanged(bool homePositionAvailable)
+{
+    _liveHomePositionAvailable = homePositionAvailable;
+    emit liveHomePositionAvailableChanged(_liveHomePositionAvailable);
+}
+
+void MissionEditor::_activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition)
+{
+    _liveHomePosition = homePosition;
+    emit liveHomePositionChanged(_liveHomePosition);
+}

src/MissionEditor/MissionEditor.h

@@ -26,6 +26,7 @@ This file is part of the QGROUNDCONTROL project
 
 #include "QGCQmlWidgetHolder.h"
 #include "QmlObjectListModel.h"
+#include "Vehicle.h"
 
 class MissionEditor : public QGCQmlWidgetHolder
 {
@@ -35,9 +36,11 @@ public:
     MissionEditor(QWidget* parent = NULL);
     ~MissionEditor();
 
-    Q_PROPERTY(QmlObjectListModel*  missionItems    READ missionItems       NOTIFY missionItemsChanged)
-    Q_PROPERTY(QmlObjectListModel*  waypointLines   READ waypointLines      NOTIFY waypointLinesChanged)
-    Q_PROPERTY(bool                 canEdit         READ canEdit            NOTIFY canEditChanged)
+    Q_PROPERTY(QmlObjectListModel*  missionItems                READ missionItems               NOTIFY missionItemsChanged)
+    Q_PROPERTY(QmlObjectListModel*  waypointLines               READ waypointLines              NOTIFY waypointLinesChanged)
+    Q_PROPERTY(bool                 canEdit                     READ canEdit                    NOTIFY canEditChanged)
+    Q_PROPERTY(bool                 liveHomePositionAvailable   READ liveHomePositionAvailable  NOTIFY liveHomePositionAvailableChanged)
+    Q_PROPERTY(QGeoCoordinate       liveHomePosition            READ liveHomePosition           NOTIFY liveHomePositionChanged)
     
     Q_INVOKABLE int addMissionItem(QGeoCoordinate coordinate);
     Q_INVOKABLE void getMissionItems(void);
@@ -53,16 +56,23 @@ public:
     QmlObjectListModel* missionItems(void) { return _missionItems; }
     QmlObjectListModel* waypointLines(void) { return &_waypointLines; }
     bool canEdit(void) { return _canEdit; }
-    
+    bool liveHomePositionAvailable(void) { return _liveHomePositionAvailable; }
+    QGeoCoordinate liveHomePosition(void) { return _liveHomePosition; }
+
 signals:
     void missionItemsChanged(void);
     void canEditChanged(bool canEdit);
     void waypointLinesChanged(void);
+    void liveHomePositionAvailableChanged(bool homePositionAvailable);
+    void liveHomePositionChanged(const QGeoCoordinate& homePosition);
     
 private slots:
     void _newMissionItemsAvailable();
     void _itemCoordinateChanged(const QGeoCoordinate& coordinate);
     void _itemCommandChanged(MavlinkQmlSingleton::Qml_MAV_CMD command);
+    void _activeVehicleChanged(Vehicle* activeVehicle);
+    void _activeVehicleHomePositionAvailableChanged(bool homePositionAvailable);
+    void _activeVehicleHomePositionChanged(const QGeoCoordinate& homePosition);
 
 private:
     void _recalcSequence(void);
@@ -78,7 +88,10 @@ private:
     QmlObjectListModel* _missionItems;
     QmlObjectListModel  _waypointLines;
     bool                _canEdit;           ///< true: UI can edit these items, false: can't edit, can only send to vehicle or save
-    
+    Vehicle*            _activeVehicle;
+    bool                _liveHomePositionAvailable;
+    QGeoCoordinate      _liveHomePosition;
+
     static const char* _settingsGroup;
 };
 

src/Vehicle/Vehicle.cc

@@ -227,8 +227,8 @@ void Vehicle::_handleHomePosition(mavlink_message_t& message)
     _homePosition.setAltitude(homePos.altitude / 1000.0);
     _homePositionAvailable = true;
     
-    emit homePositionAvailableChanged(true);
     emit homePositionChanged(_homePosition);
+    emit homePositionAvailableChanged(true);
 }
 
 void Vehicle::_handleHeartbeat(mavlink_message_t& message)
@@ -963,10 +963,6 @@ bool Vehicle::homePositionAvailable(void)
 
 QGeoCoordinate Vehicle::homePosition(void)
 {
-    if (!_homePositionAvailable) {
-        qWarning() << "Call to homePosition while _homePositionAvailable == false";
-    }
-    
     return _homePosition;
 }
 

src/comm/MockLink.cc

@@ -157,6 +157,7 @@ void MockLink::_run1HzTasks(void)
 {
     if (_mavlinkStarted && _connected) {
         _sendHeartBeat();
+        _sendHomePosition();
     }
 }
 
@@ -673,3 +674,25 @@ void MockLink::setMissionItemFailureMode(MockLinkMissionItemHandler::FailureMode
 {
     _missionItemHandler.setMissionItemFailureMode(failureMode, firstTimeOnly);
 }
+
+void MockLink::_sendHomePosition(void)
+{
+    mavlink_message_t   msg;
+    
+    float bogus[4];
+    bogus[0] = 0.0f;
+    bogus[1] = 0.0f;
+    bogus[2] = 0.0f;
+    bogus[3] = 0.0f;
+   
+    mavlink_msg_home_position_pack(_vehicleSystemId,
+                                   _vehicleComponentId,
+                                   &msg,
+                                   (int32_t)(47.633033f * 1E7),
+                                   (int32_t)(-122.08794f * 1E7),
+                                   (int32_t)(2.0f * 1000),
+                                   0.0f, 0.0f, 0.0f,
+                                   &bogus[0],
+                                   0.0f, 0.0f, 0.0f);
+    respondWithMavlinkMessage(msg);
+}

src/comm/MockLink.h

@@ -143,6 +143,7 @@ private:
     void _handleCommandLong(const mavlink_message_t& msg);
     float _floatUnionForParam(int componentId, const QString& paramName);
     void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat);
+    void _sendHomePosition(void);
 
     MockLinkMissionItemHandler  _missionItemHandler;