From 45c2e133d05d7152982e81d469a9cb3332a65615 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 22 Jun 2022 15:05:50 +0200 Subject: [PATCH] Fixes after rebase Signed-off-by: Julian Oes --- src/mavsdk/core/flight_mode.cpp | 4 +- src/mavsdk/core/flight_mode.h | 2 +- src/mavsdk/core/mavlink_mission_transfer.cpp | 3 +- src/mavsdk/core/mavlink_mission_transfer.h | 2 +- .../core/mavlink_mission_transfer_test.cpp | 3 +- src/mavsdk/core/mavlink_parameters.cpp | 16 +-- src/mavsdk/core/mocks/sender_mock.h | 2 +- src/mavsdk/core/server_component_impl.cpp | 4 +- src/mavsdk/core/server_component_impl.h | 2 +- src/mavsdk/core/system_impl.cpp | 103 +----------------- src/mavsdk/plugins/action/action_impl.cpp | 6 +- .../plugins/mission_raw/mission_import.cpp | 8 +- .../plugins/mission_raw/mission_import.h | 6 +- .../mission_raw/mission_import_test.cpp | 18 +-- .../plugins/mission_raw/mission_raw_impl.cpp | 5 +- 15 files changed, 45 insertions(+), 139 deletions(-) diff --git a/src/mavsdk/core/flight_mode.cpp b/src/mavsdk/core/flight_mode.cpp index 955b743e8f..806c5e33ee 100644 --- a/src/mavsdk/core/flight_mode.cpp +++ b/src/mavsdk/core/flight_mode.cpp @@ -8,9 +8,9 @@ namespace mavsdk { FlightMode to_flight_mode_from_custom_mode( - Sender::Autopilot autopilot, MAV_TYPE mav_type, uint32_t custom_mode) + System::CompatibilityMode compatibility_mode, MAV_TYPE mav_type, uint32_t custom_mode) { - if (autopilot == Sender::Autopilot::ArduPilot) { + if (compatibility_mode == System::CompatibilityMode::Ardupilot) { switch (mav_type) { case MAV_TYPE::MAV_TYPE_SURFACE_BOAT: case MAV_TYPE::MAV_TYPE_GROUND_ROVER: diff --git a/src/mavsdk/core/flight_mode.h b/src/mavsdk/core/flight_mode.h index 8e04b42f66..153cce22a9 100644 --- a/src/mavsdk/core/flight_mode.h +++ b/src/mavsdk/core/flight_mode.h @@ -27,7 +27,7 @@ enum class FlightMode { }; FlightMode to_flight_mode_from_custom_mode( - Sender::Autopilot autopilot, MAV_TYPE mav_type, uint32_t custom_mode); + System::CompatibilityMode compatibility_mode, MAV_TYPE mav_type, uint32_t custom_mode); FlightMode to_flight_mode_from_px4_mode(uint32_t custom_mode); FlightMode to_flight_mode_from_ardupilot_rover_mode(uint32_t custom_mode); FlightMode to_flight_mode_from_ardupilot_copter_mode(uint32_t custom_mode); diff --git a/src/mavsdk/core/mavlink_mission_transfer.cpp b/src/mavsdk/core/mavlink_mission_transfer.cpp index 4d868d6cf2..e8ae6e0ab9 100644 --- a/src/mavsdk/core/mavlink_mission_transfer.cpp +++ b/src/mavsdk/core/mavlink_mission_transfer.cpp @@ -241,7 +241,8 @@ void MavlinkMissionTransfer::UploadWorkItem::start() // item 0, and the sequence starts counting at 0 from 1. // This is only for missions, rally points, and geofence items are normal. const bool is_ardupilot_mission = - _sender.autopilot() == Sender::Autopilot::ArduPilot && _type == MAV_MISSION_TYPE_MISSION; + _sender.compatibility_mode() == System::CompatibilityMode::Ardupilot && + _type == MAV_MISSION_TYPE_MISSION; if (is_ardupilot_mission) { for (unsigned i = 1; i < _items.size(); ++i) { diff --git a/src/mavsdk/core/mavlink_mission_transfer.h b/src/mavsdk/core/mavlink_mission_transfer.h index ed66ce24dd..aab2c9b481 100644 --- a/src/mavsdk/core/mavlink_mission_transfer.h +++ b/src/mavsdk/core/mavlink_mission_transfer.h @@ -27,7 +27,7 @@ class Sender { [[nodiscard]] virtual System::CompatibilityMode compatibility_mode() const = 0; }; -class MAVLinkMissionTransfer { +class MavlinkMissionTransfer { public: enum class Result { Success, diff --git a/src/mavsdk/core/mavlink_mission_transfer_test.cpp b/src/mavsdk/core/mavlink_mission_transfer_test.cpp index 498c95a761..a80d5f75bf 100644 --- a/src/mavsdk/core/mavlink_mission_transfer_test.cpp +++ b/src/mavsdk/core/mavlink_mission_transfer_test.cpp @@ -42,7 +42,8 @@ class MavlinkMissionTransferTest : public ::testing::Test { ON_CALL(mock_sender, get_own_component_id()) .WillByDefault(Return(own_address.component_id)); ON_CALL(mock_sender, get_system_id()).WillByDefault(Return(target_address.system_id)); - ON_CALL(mock_sender, autopilot()).WillByDefault(Return(Sender::Autopilot::Px4)); + ON_CALL(mock_sender, compatibility_mode()) + .WillByDefault(Return(System::CompatibilityMode::Px4)); } MockSender mock_sender; diff --git a/src/mavsdk/core/mavlink_parameters.cpp b/src/mavsdk/core/mavlink_parameters.cpp index 5b9ab25aa5..9dc24e01af 100644 --- a/src/mavsdk/core/mavlink_parameters.cpp +++ b/src/mavsdk/core/mavlink_parameters.cpp @@ -193,7 +193,8 @@ void MAVLinkParameters::set_param_int_async( // PX4 only uses int32_t, so we can be sure and don't need to check the exact type first // by getting the param, or checking the cache. - const bool exact_int_type_known = (_sender.autopilot() == SystemImpl::Autopilot::Px4); + const bool exact_int_type_known = + (_sender.compatibility_mode() == System::CompatibilityMode::Px4); const auto set_step = [=]() { auto new_work = std::make_shared(_timeout_s_callback()); @@ -806,9 +807,10 @@ void MAVLinkParameters::do_work() param_value_buf.data(), work->param_value.get_mav_param_ext_type()); } else { - float value_set = (_sender.autopilot() == SystemImpl::Autopilot::ArduPilot) ? - work->param_value.get_4_float_bytes_cast() : - work->param_value.get_4_float_bytes_bytewise(); + float value_set = + (_sender.compatibility_mode() == System::CompatibilityMode::Ardupilot) ? + work->param_value.get_4_float_bytes_cast() : + work->param_value.get_4_float_bytes_bytewise(); mavlink_msg_param_set_pack( _sender.get_own_system_id(), @@ -914,7 +916,7 @@ void MAVLinkParameters::do_work() work->param_index); } else { float param_value; - if (_sender.autopilot() == SystemImpl::Autopilot::ArduPilot) { + if (_sender.compatibility_mode() == System::CompatibilityMode::Ardupilot) { param_value = work->param_value.get_4_float_bytes_cast(); } else { param_value = work->param_value.get_4_float_bytes_bytewise(); @@ -975,7 +977,7 @@ void MAVLinkParameters::process_param_value(const mavlink_message_t& message) } ParamValue received_value; - if (_sender.autopilot() == SystemImpl::Autopilot::ArduPilot) { + if (_sender.compatibility_mode() == System::CompatibilityMode::Ardupilot) { received_value.set_from_mavlink_param_value_cast(param_value); } else { received_value.set_from_mavlink_param_value_bytewise(param_value); @@ -1098,7 +1100,7 @@ void MAVLinkParameters::notify_param_subscriptions(const mavlink_param_value_t& ParamValue value; - if (_sender.autopilot() == SystemImpl::Autopilot::ArduPilot) { + if (_sender.compatibility_mode() == System::CompatibilityMode::Ardupilot) { value.set_from_mavlink_param_value_cast(param_value); } else { value.set_from_mavlink_param_value_bytewise(param_value); diff --git a/src/mavsdk/core/mocks/sender_mock.h b/src/mavsdk/core/mocks/sender_mock.h index da0f4a1fd4..940d447ccb 100644 --- a/src/mavsdk/core/mocks/sender_mock.h +++ b/src/mavsdk/core/mocks/sender_mock.h @@ -11,7 +11,7 @@ class MockSender : public Sender { MOCK_METHOD(uint8_t, get_own_system_id, (), (const, override)); MOCK_METHOD(uint8_t, get_own_component_id, (), (const, override)); MOCK_METHOD(uint8_t, get_system_id, (), (const, override)); - MOCK_METHOD(Autopilot, autopilot, (), (const, override)); + MOCK_METHOD(System::CompatibilityMode, compatibility_mode, (), (const, override)); }; } // namespace testing diff --git a/src/mavsdk/core/server_component_impl.cpp b/src/mavsdk/core/server_component_impl.cpp index c2728d924c..b42c452f4a 100644 --- a/src/mavsdk/core/server_component_impl.cpp +++ b/src/mavsdk/core/server_component_impl.cpp @@ -382,10 +382,10 @@ uint8_t ServerComponentImpl::OurSender::get_system_id() const return current_target_system_id; } -Sender::Autopilot ServerComponentImpl::OurSender::autopilot() const +System::CompatibilityMode ServerComponentImpl::OurSender::compatibility_mode() const { // FIXME: hard-coded to PX4 for now to avoid the dependency into mavsdk_impl. - return Sender::Autopilot::Px4; + return System::CompatibilityMode::Px4; } } // namespace mavsdk diff --git a/src/mavsdk/core/server_component_impl.h b/src/mavsdk/core/server_component_impl.h index adfad82ba0..c10e607120 100644 --- a/src/mavsdk/core/server_component_impl.h +++ b/src/mavsdk/core/server_component_impl.h @@ -34,7 +34,7 @@ class ServerComponentImpl { [[nodiscard]] uint8_t get_own_system_id() const override; [[nodiscard]] uint8_t get_own_component_id() const override; [[nodiscard]] uint8_t get_system_id() const override; - [[nodiscard]] Autopilot autopilot() const override; + [[nodiscard]] System::CompatibilityMode compatibility_mode() const override; uint8_t current_target_system_id{0}; diff --git a/src/mavsdk/core/system_impl.cpp b/src/mavsdk/core/system_impl.cpp index afd6ccf868..03f1406509 100644 --- a/src/mavsdk/core/system_impl.cpp +++ b/src/mavsdk/core/system_impl.cpp @@ -247,8 +247,8 @@ void SystemImpl::process_heartbeat(const mavlink_message_t& message) _hitl_enabled = (heartbeat.base_mode & MAV_MODE_FLAG_HIL_ENABLED) != 0; } if (heartbeat.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { - _flight_mode = - to_flight_mode_from_custom_mode(_autopilot, _vehicle_type, heartbeat.custom_mode); + _flight_mode = to_flight_mode_from_custom_mode( + _compatibility_mode, _vehicle_type, heartbeat.custom_mode); } set_connected(); @@ -1060,105 +1060,6 @@ FlightMode SystemImpl::get_flight_mode() const return _flight_mode; } -SystemImpl::FlightMode SystemImpl::to_flight_mode_from_custom_mode(uint32_t custom_mode) -{ - if (compatibility_mode() == System::CompatibilityMode::Ardupilot) { - switch (_vehicle_type) { - case MAV_TYPE::MAV_TYPE_SURFACE_BOAT: - case MAV_TYPE::MAV_TYPE_GROUND_ROVER: - return to_flight_mode_from_ardupilot_rover_mode(custom_mode); - default: - return to_flight_mode_from_ardupilot_copter_mode(custom_mode); - } - } else { - return to_flight_mode_from_px4_mode(custom_mode); - } -} - -SystemImpl::FlightMode SystemImpl::to_flight_mode_from_ardupilot_rover_mode(uint32_t custom_mode) -{ - switch (static_cast(custom_mode)) { - case ardupilot::RoverMode::Auto: - return FlightMode::Mission; - case ardupilot::RoverMode::Acro: - return FlightMode::Acro; - case ardupilot::RoverMode::Hold: - return FlightMode::Hold; - case ardupilot::RoverMode::RTL: - return FlightMode::ReturnToLaunch; - case ardupilot::RoverMode::Manual: - return FlightMode::Manual; - case ardupilot::RoverMode::Follow: - return FlightMode::FollowMe; - default: - return FlightMode::Unknown; - } -} -SystemImpl::FlightMode SystemImpl::to_flight_mode_from_ardupilot_copter_mode(uint32_t custom_mode) -{ - switch (static_cast(custom_mode)) { - case ardupilot::CopterMode::Auto: - return FlightMode::Mission; - case ardupilot::CopterMode::Acro: - return FlightMode::Acro; - case ardupilot::CopterMode::Alt_Hold: - case ardupilot::CopterMode::POS_HOLD: - case ardupilot::CopterMode::Flow_Hold: - return FlightMode::Hold; - case ardupilot::CopterMode::RTL: - case ardupilot::CopterMode::Auto_RTL: - return FlightMode::ReturnToLaunch; - case ardupilot::CopterMode::Land: - return FlightMode::Land; - default: - return FlightMode::Unknown; - } -} - -SystemImpl::FlightMode SystemImpl::to_flight_mode_from_px4_mode(uint32_t custom_mode) -{ - px4::px4_custom_mode px4_custom_mode; - px4_custom_mode.data = custom_mode; - - switch (px4_custom_mode.main_mode) { - case px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD: - return FlightMode::Offboard; - case px4::PX4_CUSTOM_MAIN_MODE_MANUAL: - return FlightMode::Manual; - case px4::PX4_CUSTOM_MAIN_MODE_POSCTL: - return FlightMode::Posctl; - case px4::PX4_CUSTOM_MAIN_MODE_ALTCTL: - return FlightMode::Altctl; - case px4::PX4_CUSTOM_MAIN_MODE_RATTITUDE: - return FlightMode::Rattitude; - case px4::PX4_CUSTOM_MAIN_MODE_ACRO: - return FlightMode::Acro; - case px4::PX4_CUSTOM_MAIN_MODE_STABILIZED: - return FlightMode::Stabilized; - case px4::PX4_CUSTOM_MAIN_MODE_AUTO: - switch (px4_custom_mode.sub_mode) { - case px4::PX4_CUSTOM_SUB_MODE_AUTO_READY: - return FlightMode::Ready; - case px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF: - return FlightMode::Takeoff; - case px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER: - return FlightMode::Hold; - case px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION: - return FlightMode::Mission; - case px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL: - return FlightMode::ReturnToLaunch; - case px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND: - return FlightMode::Land; - case px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET: - return FlightMode::FollowMe; - default: - return FlightMode::Unknown; - } - default: - return FlightMode::Unknown; - } -} - void SystemImpl::receive_float_param( MAVLinkParameters::Result result, MAVLinkParameters::ParamValue value, diff --git a/src/mavsdk/plugins/action/action_impl.cpp b/src/mavsdk/plugins/action/action_impl.cpp index 8b6d48498d..3ccc23dc50 100644 --- a/src/mavsdk/plugins/action/action_impl.cpp +++ b/src/mavsdk/plugins/action/action_impl.cpp @@ -364,7 +364,7 @@ void ActionImpl::shutdown_async(const Action::ResultCallback& callback) const void ActionImpl::takeoff_async(const Action::ResultCallback& callback) const { - if (_parent->autopilot() == SystemImpl::Autopilot::Px4) { + if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) { takeoff_async_px4(callback); } else { takeoff_async_apm(callback); @@ -466,9 +466,9 @@ void ActionImpl::goto_location_async( if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) { // Change to Hold mode first - if (_parent->get_flight_mode() != SystemImpl::FlightMode::Hold) { + if (_parent->get_flight_mode() != FlightMode::Hold) { _parent->set_flight_mode_async( - SystemImpl::FlightMode::Hold, + FlightMode::Hold, [this, callback, send_do_reposition](MavlinkCommandSender::Result result, float) { Action::Result action_result = action_result_from_command_result(result); if (action_result != Action::Result::Success) { diff --git a/src/mavsdk/plugins/mission_raw/mission_import.cpp b/src/mavsdk/plugins/mission_raw/mission_import.cpp index daa61029de..a0286a62e7 100644 --- a/src/mavsdk/plugins/mission_raw/mission_import.cpp +++ b/src/mavsdk/plugins/mission_raw/mission_import.cpp @@ -7,7 +7,7 @@ namespace mavsdk { std::pair -MissionImport::parse_json(const std::string& raw_json, Sender::Autopilot autopilot) +MissionImport::parse_json(const std::string& raw_json, System::CompatibilityMode compatibility_mode) { Json::CharReaderBuilder builder; const std::unique_ptr reader(builder.newCharReader()); @@ -23,7 +23,7 @@ MissionImport::parse_json(const std::string& raw_json, Sender::Autopilot autopil return {MissionRaw::Result::FailedToParseQgcPlan, {}}; } - auto maybe_mission_items = import_mission(root, autopilot); + auto maybe_mission_items = import_mission(root, compatibility_mode); if (!maybe_mission_items.has_value()) { return {MissionRaw::Result::FailedToParseQgcPlan, {}}; } @@ -60,7 +60,7 @@ bool MissionImport::check_overall_version(const Json::Value& root) } std::optional> -MissionImport::import_mission(const Json::Value& root, Sender::Autopilot autopilot) +MissionImport::import_mission(const Json::Value& root, System::CompatibilityMode compatibility_mode) { // We need a mission part. const auto mission = root["mission"]; @@ -119,7 +119,7 @@ MissionImport::import_mission(const Json::Value& root, Sender::Autopilot autopil } // Add home position at 0 for ArduPilot - if (autopilot == Sender::Autopilot::ArduPilot) { + if (compatibility_mode == System::CompatibilityMode::Ardupilot) { const auto home = mission["plannedHomePosition"]; if (!home.empty()) { if (home.isArray() && home.size() != 3) { diff --git a/src/mavsdk/plugins/mission_raw/mission_import.h b/src/mavsdk/plugins/mission_raw/mission_import.h index 41bb796ec1..c9b8fffa9f 100644 --- a/src/mavsdk/plugins/mission_raw/mission_import.h +++ b/src/mavsdk/plugins/mission_raw/mission_import.h @@ -1,7 +1,7 @@ #pragma once #include "plugins/mission_raw/mission_raw.h" -#include "sender.h" +#include "system.h" #include #include #include @@ -12,7 +12,7 @@ namespace mavsdk { class MissionImport { public: static std::pair - parse_json(const std::string& raw_json, Sender::Autopilot autopilot); + parse_json(const std::string& raw_json, System::CompatibilityMode compatibility_mode); private: static bool check_overall_version(const Json::Value& root); @@ -21,7 +21,7 @@ class MissionImport { static std::optional> import_rally_points(const Json::Value& root); static std::optional> - import_mission(const Json::Value& root, Sender::Autopilot autopilot); + import_mission(const Json::Value& root, System::CompatibilityMode compatibility_mode); static std::optional import_simple_mission_item(const Json::Value& json_item); static std::optional> diff --git a/src/mavsdk/plugins/mission_raw/mission_import_test.cpp b/src/mavsdk/plugins/mission_raw/mission_import_test.cpp index 5b3042b3cc..0e05bd20c6 100644 --- a/src/mavsdk/plugins/mission_raw/mission_import_test.cpp +++ b/src/mavsdk/plugins/mission_raw/mission_import_test.cpp @@ -112,7 +112,7 @@ TEST(MissionRaw, ImportSamplePlanSuccessfully) buf << file.rdbuf(); file.close(); - const auto result_pair = MissionImport::parse_json(buf.str(), Sender::Autopilot::Px4); + const auto result_pair = MissionImport::parse_json(buf.str(), System::CompatibilityMode::Px4); ASSERT_EQ(result_pair.first, MissionRaw::Result::Success); EXPECT_EQ(reference_items, result_pair.second.mission_items); @@ -127,7 +127,7 @@ TEST(MissionRaw, ImportSamplePlanWithWrongOverallVersion) buf << file.rdbuf(); file.close(); - const auto result_pair = MissionImport::parse_json(buf.str(), Sender::Autopilot::Px4); + const auto result_pair = MissionImport::parse_json(buf.str(), System::CompatibilityMode::Px4); EXPECT_EQ(result_pair.first, MissionRaw::Result::FailedToParseQgcPlan); EXPECT_EQ(result_pair.second.mission_items.size(), 0); EXPECT_EQ(result_pair.second.geofence_items.size(), 0); @@ -143,7 +143,7 @@ TEST(MissionRaw, ImportSamplePlanWithWrongMissionVersion) buf << file.rdbuf(); file.close(); - const auto result_pair = MissionImport::parse_json(buf.str(), Sender::Autopilot::Px4); + const auto result_pair = MissionImport::parse_json(buf.str(), System::CompatibilityMode::Px4); EXPECT_EQ(result_pair.first, MissionRaw::Result::FailedToParseQgcPlan); EXPECT_EQ(result_pair.second.mission_items.size(), 0); EXPECT_EQ(result_pair.second.geofence_items.size(), 0); @@ -159,7 +159,7 @@ TEST(MissionRaw, ImportSamplePlanWithoutMission) buf << file.rdbuf(); file.close(); - const auto result_pair = MissionImport::parse_json(buf.str(), Sender::Autopilot::Px4); + const auto result_pair = MissionImport::parse_json(buf.str(), System::CompatibilityMode::Px4); EXPECT_EQ(result_pair.first, MissionRaw::Result::FailedToParseQgcPlan); EXPECT_EQ(result_pair.second.mission_items.size(), 0); EXPECT_EQ(result_pair.second.geofence_items.size(), 0); @@ -346,7 +346,7 @@ TEST(MissionRaw, ImportSamplePlanWithComplexMissionSurvey) buf << file.rdbuf(); file.close(); - const auto result_pair = MissionImport::parse_json(buf.str(), Sender::Autopilot::Px4); + const auto result_pair = MissionImport::parse_json(buf.str(), System::CompatibilityMode::Px4); EXPECT_EQ(result_pair.first, MissionRaw::Result::Success); EXPECT_EQ(reference_items, result_pair.second.mission_items); @@ -363,7 +363,7 @@ TEST(MissionRaw, ImportSamplePlanWithComplexMissionSurveyWrongVersion) buf << file.rdbuf(); file.close(); - const auto result_pair = MissionImport::parse_json(buf.str(), Sender::Autopilot::Px4); + const auto result_pair = MissionImport::parse_json(buf.str(), System::CompatibilityMode::Px4); EXPECT_EQ(result_pair.first, MissionRaw::Result::FailedToParseQgcPlan); EXPECT_EQ(result_pair.second.mission_items.size(), 0); EXPECT_EQ(result_pair.second.geofence_items.size(), 0); @@ -379,7 +379,7 @@ TEST(MissionRaw, ImportSamplePlanWithComplexMissionStructuredScan) buf << file.rdbuf(); file.close(); - const auto result_pair = MissionImport::parse_json(buf.str(), Sender::Autopilot::Px4); + const auto result_pair = MissionImport::parse_json(buf.str(), System::CompatibilityMode::Px4); EXPECT_EQ(result_pair.first, MissionRaw::Result::FailedToParseQgcPlan); EXPECT_EQ(result_pair.second.mission_items.size(), 0); EXPECT_EQ(result_pair.second.geofence_items.size(), 0); @@ -395,7 +395,7 @@ TEST(MissionRaw, ImportSamplePlanWithComplexMissionSurveyMissingItems) buf << file.rdbuf(); file.close(); - const auto result_pair = MissionImport::parse_json(buf.str(), Sender::Autopilot::Px4); + const auto result_pair = MissionImport::parse_json(buf.str(), System::CompatibilityMode::Px4); EXPECT_EQ(result_pair.first, MissionRaw::Result::FailedToParseQgcPlan); EXPECT_EQ(result_pair.second.mission_items.size(), 0); EXPECT_EQ(result_pair.second.geofence_items.size(), 0); @@ -433,7 +433,7 @@ TEST(MissionRaw, ImportSamplePlanWithArduPilot) buf << file.rdbuf(); file.close(); - const auto result_pair = MissionImport::parse_json(buf.str(), Sender::Autopilot::ArduPilot); + const auto result_pair = MissionImport::parse_json(buf.str(), System::CompatibilityMode::Px4); ASSERT_EQ(result_pair.first, MissionRaw::Result::Success); EXPECT_EQ(reference_items, result_pair.second.mission_items); diff --git a/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp b/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp index bf98905ade..7362cde1ad 100644 --- a/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp +++ b/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp @@ -521,7 +521,8 @@ MissionRawImpl::subscribe_mission_changed(const MissionRaw::MissionChangedCallba // will be received. This should be specced properly, hence it's not in Pure mode. if (_parent->compatibility_mode() == System::CompatibilityMode::Pure) { LogErr() << "mission changed subscription not supported"; - return; + // We can't really signal this to the caller except doing an abort which would + // be a bit brutal, so we just let the caller get a subscription anyway. } std::lock_guard lock(_mission_changed.mutex); @@ -547,7 +548,7 @@ MissionRawImpl::import_qgroundcontrol_mission(std::string qgc_plan_path) buf << file.rdbuf(); file.close(); - return MissionImport::parse_json(buf.str(), _parent->autopilot()); + return MissionImport::parse_json(buf.str(), _parent->compatibility_mode()); } MissionRaw::Result MissionRawImpl::convert_result(MavlinkMissionTransfer::Result result)