diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index 471cd1d94d6c5847eb446f056a657da16e4284a5..38b0598ebdbf1ca18e3181cea8702e648473fc9c 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -483,19 +483,22 @@ void MissionController::_initAllMissionItems(void) } homeItem->setHomePositionSpecialCase(true); if (_activeVehicle) { - homeItem->setCoordinate(_activeVehicle->homePosition()); homeItem->setHomePositionValid(_activeVehicle->homePositionAvailable()); + if (homeItem->homePositionValid()) { + homeItem->setCoordinate(_activeVehicle->homePosition()); + } } else { homeItem->setHomePositionValid(false); } homeItem->setCommand(MAV_CMD_NAV_WAYPOINT); homeItem->setFrame(MAV_FRAME_GLOBAL); if (!homeItem->homePositionValid()) { - QGeoCoordinate homeCoord = homeItem->coordinate(); - homeCoord.setAltitude(0.0); - homeItem->setCoordinate(homeCoord); + // Set a bogus home position, the important value is 0.0 Altitude + homeItem->setCoordinate(QGeoCoordinate(37.803784, -122.462276, 0.0)); } + qDebug() << "home item" << homeItem->homePositionValid() << homeItem->coordinate(); + for (int i=0; i<_missionItems->count(); i++) { _initMissionItem(qobject_cast(_missionItems->get(i))); }