Merge pull request #2352 from DonLakeFlyer/HomePosFixes

Home position fixes

src/MissionManager/MissionController.cc

@@ -426,21 +426,21 @@ void MissionController::_initAllMissionItems(void)
 
     if (_activeVehicle && _activeVehicle->firmwarePlugin()->sendHomePositionToVehicle() && _missionItems->count() != 0) {
         homeItem = qobject_cast<MissionItem*>(_missionItems->get(0));
-        homeItem->setHomePositionSpecialCase(true);
     } else {
         // Add the home position item to the front
         homeItem = new MissionItem(this);
-        homeItem->setHomePositionSpecialCase(true);
-        if (_activeVehicle) {
-            homeItem->setCoordinate(_activeVehicle->homePosition());
-            homeItem->setHomePositionValid(_activeVehicle->homePositionAvailable());
-        }
-        homeItem->setCommand(MAV_CMD_NAV_WAYPOINT);
-        homeItem->setFrame(MAV_FRAME_GLOBAL_RELATIVE_ALT);
         homeItem->setSequenceNumber(0);
         _missionItems->insert(0, homeItem);
     }
-    homeItem->setHomePositionValid(false);
+    homeItem->setHomePositionSpecialCase(true);
+    if (_activeVehicle) {
+        homeItem->setCoordinate(_activeVehicle->homePosition());
+        homeItem->setHomePositionValid(_activeVehicle->homePositionAvailable());
+    } else {
+        homeItem->setHomePositionValid(false);
+    }
+    homeItem->setCommand(MAV_CMD_NAV_WAYPOINT);
+    homeItem->setFrame(MAV_FRAME_GLOBAL);
 
     for (int i=0; i<_missionItems->count(); i++) {
         _initMissionItem(qobject_cast<MissionItem*>(_missionItems->get(i)));

src/Vehicle/Vehicle.cc

@@ -236,17 +236,18 @@ void Vehicle::_handleHomePosition(mavlink_message_t& message)
     QGeoCoordinate newHomePosition (homePos.latitude / 10000000.0,
                                     homePos.longitude / 10000000.0,
                                     homePos.altitude / 1000.0);
-    if (newHomePosition != _homePosition) {
+    if (!_homePositionAvailable || newHomePosition != _homePosition) {
         emitHomePositionChanged = true;
         _homePosition = newHomePosition;
     }
 
     if (!_homePositionAvailable) {
         emitHomePositionAvailableChanged = true;
+        _homePositionAvailable = true;
     }
-    _homePositionAvailable = true;
 
     if (emitHomePositionChanged) {
+        qCDebug(VehicleLog) << "New home position" << newHomePosition;
         emit homePositionChanged(_homePosition);
     }
     if (emitHomePositionAvailableChanged) {

src/comm/MockLink.cc

@@ -70,7 +70,7 @@ union px4_custom_mode {
 
 float MockLink::_vehicleLatitude =  47.633033f;
 float MockLink::_vehicleLongitude = -122.08794f;
-float MockLink::_vehicleAltitude =  9872.5f;
+float MockLink::_vehicleAltitude =  3.5f;
 
 const char* MockConfiguration::_firmwareTypeKey =   "FirmwareType";
 const char* MockConfiguration::_vehicleTypeKey =    "VehicleType";