diff --git a/src/MissionManager/MissionController.cc b/src/MissionManager/MissionController.cc index b00d9699e3dd74d91f3b727674e7ac9ef525b686..11c40c2c59dc0e1ff50c3ac41b31571c2ab4edb8 100644 --- a/src/MissionManager/MissionController.cc +++ b/src/MissionManager/MissionController.cc @@ -158,7 +158,10 @@ void MissionController::_newMissionItemsAvailableFromVehicle(bool removeAllReque qWarning() << "First item is not settings item"; return; } - settingsItem->setCoordinate(newMissionItems[0]->coordinate()); + MissionItem* fakeHomeItem = newMissionItems[0]; + if (fakeHomeItem->coordinate().latitude() != 0 || fakeHomeItem->coordinate().longitude() != 0) { + settingsItem->setCoordinate(fakeHomeItem->coordinate()); + } i = 1; } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 242e647c9dac4e9bfc233edae26e41cb205d828b..521d7d06287bb0c304d3588e7af5e1f08014317d 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -681,8 +681,8 @@ void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message) mavlink_global_position_int_t globalPositionInt; mavlink_msg_global_position_int_decode(&message, &globalPositionInt); - // ArduPilot sends bogus GLOBAL_POSITION_INT messages with lat/lat/alt 0/0/0 even when it has not gps signal - if (globalPositionInt.lat == 0 && globalPositionInt.lon == 0 && globalPositionInt.alt == 0) { + // ArduPilot sends bogus GLOBAL_POSITION_INT messages with lat/lat 0/0 even when it has not gps signal + if (globalPositionInt.lat == 0 && globalPositionInt.lon == 0) { return; }