diff --git a/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h b/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h index 7816f5656f..ce2c9ad8c5 100644 --- a/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h +++ b/src/mavsdk/plugins/mission_raw/include/plugins/mission_raw/mission_raw.h @@ -124,6 +124,7 @@ class MissionRaw : public PluginBase { std::vector mission_items{}; /**< @brief Mission items */ std::vector geofence_items{}; /**< @brief Geofence items */ std::vector rally_items{}; /**< @brief Rally items */ + std::optional plannedHomePosition; }; /** diff --git a/src/mavsdk/plugins/mission_raw/mission_import.cpp b/src/mavsdk/plugins/mission_raw/mission_import.cpp index 17bff7240b..08a82a6990 100644 --- a/src/mavsdk/plugins/mission_raw/mission_import.cpp +++ b/src/mavsdk/plugins/mission_raw/mission_import.cpp @@ -24,12 +24,14 @@ MissionImport::parse_json(const std::string& raw_json, Sender::Autopilot autopil } auto maybe_mission_items = import_mission(root, autopilot); - if (!maybe_mission_items.has_value()) { + if (!maybe_mission_items.first.has_value()) { return {MissionRaw::Result::FailedToParseQgcPlan, {}}; } MissionRaw::MissionImportData import_data; - import_data.mission_items = maybe_mission_items.value(); + import_data.mission_items = maybe_mission_items.first.value(); + import_data.plannedHomePosition = maybe_mission_items.second.value(); + return {MissionRaw::Result::Success, import_data}; } @@ -46,14 +48,14 @@ bool MissionImport::check_overall_version(const Json::Value& root) return true; } -std::optional> +std::pair>, std::optional> MissionImport::import_mission(const Json::Value& root, Sender::Autopilot autopilot) { // We need a mission part. const auto mission = root["mission"]; if (mission.empty()) { LogErr() << "No mission found in .plan."; - return std::nullopt; + return std::make_pair(std::nullopt, std::nullopt); } // Check the mission version. @@ -62,11 +64,36 @@ MissionImport::import_mission(const Json::Value& root, Sender::Autopilot autopil if (mission_version.empty() || mission_version.asInt() != supported_mission_version) { LogErr() << "mission version for .plan not supported, found version: " << mission_version.asInt() << ", supported: " << supported_mission_version; - return std::nullopt; + return std::make_pair(std::nullopt, std::nullopt); } std::vector mission_items; + const auto home = mission["plannedHomePosition"]; + std::optional home_item; + + if (!home.empty()) { + if (home.isArray() && home.size() != 3) { + LogErr() << "Unknown plannedHomePosition format"; + return std::make_pair(std::nullopt, std::nullopt); + } + + home_item = MissionRaw::MissionItem{ + 0, + MAV_FRAME_GLOBAL_INT, + MAV_CMD_NAV_WAYPOINT, + 0, // current + 1, // autocontinue + 0.0f, + 0.0f, + 0.0f, + 0.0f, + static_cast(std::round(home[0].asDouble() * 1e7)), + static_cast(std::round(home[1].asDouble() * 1e7)), + home[2].asFloat(), + MAV_MISSION_TYPE_MISSION}; + } + // Go through items for (const auto& json_item : mission["items"]) { const auto type = json_item["type"]; @@ -76,7 +103,7 @@ MissionImport::import_mission(const Json::Value& root, Sender::Autopilot autopil if (maybe_item.has_value()) { mission_items.push_back(maybe_item.value()); } else { - return std::nullopt; + return std::make_pair(std::nullopt, home_item); } } else if (!type.isNull() && type.asString() == "ComplexItem") { @@ -85,12 +112,12 @@ MissionImport::import_mission(const Json::Value& root, Sender::Autopilot autopil mission_items.insert( mission_items.end(), maybe_items.value().begin(), maybe_items.value().end()); } else { - return std::nullopt; + return std::make_pair(std::nullopt, home_item); } } else { LogErr() << "Type " << type.asString() << " not understood."; - return std::nullopt; + return std::make_pair(std::nullopt, home_item); } } @@ -106,35 +133,14 @@ MissionImport::import_mission(const Json::Value& root, Sender::Autopilot autopil } // Add home position at 0 for ArduPilot - if (autopilot == Sender::Autopilot::ArduPilot) { - const auto home = mission["plannedHomePosition"]; - if (!home.empty()) { - if (home.isArray() && home.size() != 3) { - LogErr() << "Unknown plannedHomePosition format"; - return std::nullopt; - } - + if (autopilot == Sender::Autopilot::ArduPilot && home_item.has_value()) { mission_items.insert( mission_items.begin(), - MissionRaw::MissionItem{ - 0, - MAV_FRAME_GLOBAL_INT, - MAV_CMD_NAV_WAYPOINT, - 0, // current - 1, // autocontinue - 0.0f, - 0.0f, - 0.0f, - 0.0f, - static_cast(std::round(home[0].asDouble() * 1e7)), - static_cast(std::round(home[1].asDouble() * 1e7)), - home[2].asFloat(), - MAV_MISSION_TYPE_MISSION}); - } + home_item.value()); } // Returning an empty vector is ok here if there were really no mission items. - return {mission_items}; + return std::make_pair(mission_items, home_item); } std::optional diff --git a/src/mavsdk/plugins/mission_raw/mission_import.h b/src/mavsdk/plugins/mission_raw/mission_import.h index 406428de81..35f0b52b87 100644 --- a/src/mavsdk/plugins/mission_raw/mission_import.h +++ b/src/mavsdk/plugins/mission_raw/mission_import.h @@ -16,7 +16,7 @@ class MissionImport { private: static bool check_overall_version(const Json::Value& root); - static std::optional> + static std::pair>, std::optional> import_mission(const Json::Value& root, SystemImpl::Autopilot autopilot); static std::optional import_simple_mission_item(const Json::Value& json_item);