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/include/mavsdk/system.h b/src/mavsdk/core/include/mavsdk/system.h index 7cda23ab2c..906a531d32 100644 --- a/src/mavsdk/core/include/mavsdk/system.h +++ b/src/mavsdk/core/include/mavsdk/system.h @@ -183,6 +183,26 @@ class System { */ void enable_timesync(); + /** + * @brief Possible compatibility modes for this system. + */ + enum class CompatibilityMode { + Unknown, // Unknown, not set or detected yet. + Pure, // No compatibility, everything up to common spec. + Px4, // Compatibility with PX4. + Ardupilot, // Compatibility with ArduPilot. + }; + + friend std::ostream& operator<<(std::ostream& str, CompatibilityMode const& compatibility_mode); + + /** + * @brief Set compatibility mode. + * + * This disables auto-detection based on the heartbeat and sticks to + * set compatibility mode. + */ + void set_compatibility_mode(CompatibilityMode compatibility_mode); + /** * @brief Copy constructor (object is not copyable). */ diff --git a/src/mavsdk/core/mavlink_command_sender.cpp b/src/mavsdk/core/mavlink_command_sender.cpp index ea8c1da623..d8ef3dc066 100644 --- a/src/mavsdk/core/mavlink_command_sender.cpp +++ b/src/mavsdk/core/mavlink_command_sender.cpp @@ -471,7 +471,7 @@ float MavlinkCommandSender::maybe_reserved(const std::optional& maybe_par return maybe_param.value(); } else { - if (_parent.autopilot() == SystemImpl::Autopilot::ArduPilot) { + if (_parent.compatibility_mode() == System::CompatibilityMode::Ardupilot) { return 0.0f; } else { return NAN; diff --git a/src/mavsdk/core/mavlink_mission_transfer.cpp b/src/mavsdk/core/mavlink_mission_transfer.cpp index d17451df0d..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) { @@ -346,7 +347,7 @@ void MavlinkMissionTransfer::UploadWorkItem::send_cancel_and_finish() void MavlinkMissionTransfer::UploadWorkItem::process_mission_request( const mavlink_message_t& request_message) { - if (_sender.autopilot() == Sender::Autopilot::ArduPilot) { + if (_sender.compatibility_mode() == System::CompatibilityMode::Ardupilot) { // ArduCopter 3.6 sends MISSION_REQUEST (not _INT) but actually accepts ITEM_INT in reply mavlink_mission_request_t request; mavlink_msg_mission_request_decode(&request_message, &request); diff --git a/src/mavsdk/core/mavlink_mission_transfer.h b/src/mavsdk/core/mavlink_mission_transfer.h index 0730a184a5..aab2c9b481 100644 --- a/src/mavsdk/core/mavlink_mission_transfer.h +++ b/src/mavsdk/core/mavlink_mission_transfer.h @@ -12,10 +12,21 @@ #include "timeout_handler.h" #include "timeout_s_callback.h" #include "locked_queue.h" -#include "sender.h" +#include "mavsdk.h" namespace mavsdk { +class Sender { +public: + Sender() = default; + virtual ~Sender() = default; + virtual bool send_message(mavlink_message_t& message) = 0; + [[nodiscard]] virtual uint8_t get_own_system_id() const = 0; + [[nodiscard]] virtual uint8_t get_own_component_id() const = 0; + [[nodiscard]] virtual uint8_t get_system_id() const = 0; + [[nodiscard]] virtual System::CompatibilityMode compatibility_mode() const = 0; +}; + class MavlinkMissionTransfer { public: enum class Result { 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.cpp b/src/mavsdk/core/system.cpp index 255eb111f4..87ee76d827 100644 --- a/src/mavsdk/core/system.cpp +++ b/src/mavsdk/core/system.cpp @@ -92,4 +92,25 @@ void System::enable_timesync() _system_impl->enable_timesync(); } +void System::set_compatibility_mode(CompatibilityMode compatibility_mode) +{ + _system_impl->set_compatibility_mode(compatibility_mode); +} + +std::ostream& operator<<(std::ostream& str, const System::CompatibilityMode& compatibility_mode) +{ + switch (compatibility_mode) { + default: + // Passthrough + case System::CompatibilityMode::Unknown: + return str << "Unknown"; + case System::CompatibilityMode::Pure: + return str << "Pure"; + case System::CompatibilityMode::Px4: + return str << "PX4"; + case System::CompatibilityMode::Ardupilot: + return str << "ArduPilot"; + } +} + } // namespace mavsdk diff --git a/src/mavsdk/core/system_impl.cpp b/src/mavsdk/core/system_impl.cpp index 496b739d58..03f1406509 100644 --- a/src/mavsdk/core/system_impl.cpp +++ b/src/mavsdk/core/system_impl.cpp @@ -214,10 +214,17 @@ void SystemImpl::process_heartbeat(const mavlink_message_t& message) mavlink_heartbeat_t heartbeat; mavlink_msg_heartbeat_decode(&message, &heartbeat); - if (heartbeat.autopilot == MAV_AUTOPILOT_PX4) { - _autopilot = Autopilot::Px4; - } else if (heartbeat.autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA) { - _autopilot = Autopilot::ArduPilot; + { + std::lock_guard lock(_statustext_handler_callbacks_mutex); + if (!_compatibility_mode_fixed) { + if (heartbeat.autopilot == MAV_AUTOPILOT_PX4) { + _compatibility_mode = System::CompatibilityMode::Px4; + LogDebug() << "Auto-detected " << _compatibility_mode; + } else if (heartbeat.autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA) { + _compatibility_mode = System::CompatibilityMode::Ardupilot; + LogDebug() << "Auto-detected " << _compatibility_mode; + } + } } // Only set the vehicle type if the heartbeat is from an autopilot component @@ -240,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(); @@ -812,7 +819,7 @@ void SystemImpl::set_flight_mode_async( std::pair SystemImpl::make_command_flight_mode(FlightMode flight_mode, uint8_t component_id) { - if (_autopilot == Autopilot::ArduPilot) { + if (compatibility_mode() == System::CompatibilityMode::Ardupilot) { return make_command_ardupilot_mode(flight_mode, component_id); } else { return make_command_px4_mode(flight_mode, component_id); @@ -1306,4 +1313,18 @@ void SystemImpl::subscribe_param_custom( _params.subscribe_param_custom_changed(name, callback, cookie); } +void SystemImpl::set_compatibility_mode(System::CompatibilityMode compatibility_mode) +{ + std::lock_guard lock(_compatibility_mode_mutex); + _compatibility_mode = compatibility_mode; + + if (compatibility_mode == System::CompatibilityMode::Unknown) { + LogDebug() << "Compatibility reset to Unknown/automatic"; + _compatibility_mode_fixed = false; + } else { + LogDebug() << "Compatibility fixed to " << compatibility_mode; + _compatibility_mode_fixed = true; + } +} + } // namespace mavsdk diff --git a/src/mavsdk/core/system_impl.h b/src/mavsdk/core/system_impl.h index ad293bb8eb..183b6cb967 100644 --- a/src/mavsdk/core/system_impl.h +++ b/src/mavsdk/core/system_impl.h @@ -77,7 +77,7 @@ class SystemImpl : public Sender { bool send_message(mavlink_message_t& message) override; - Autopilot autopilot() const override { return _autopilot; }; + System::CompatibilityMode compatibility_mode() const override { return _compatibility_mode; }; using CommandResultCallback = MavlinkCommandSender::CommandResultCallback; @@ -274,12 +274,14 @@ class SystemImpl : public Sender { RequestMessage& request_message() { return _request_message; }; + double timeout_s() const; + + void set_compatibility_mode(System::CompatibilityMode compatibility_mode); + // Non-copyable SystemImpl(const SystemImpl&) = delete; const SystemImpl& operator=(const SystemImpl&) = delete; - double timeout_s() const; - private: static bool is_autopilot(uint8_t comp_id); static bool is_camera(uint8_t comp_id); @@ -346,8 +348,6 @@ class SystemImpl : public Sender { std::atomic _hitl_enabled{false}; bool _always_connected{false}; - std::atomic _autopilot{Autopilot::Unknown}; - MavsdkImpl& _parent; std::thread* _system_thread{nullptr}; @@ -392,6 +392,10 @@ class SystemImpl : public Sender { bool _old_message_520_supported{true}; bool _old_message_528_supported{true}; + + std::mutex _compatibility_mode_mutex{}; + System::CompatibilityMode _compatibility_mode{System::CompatibilityMode::Unknown}; + bool _compatibility_mode_fixed{false}; }; } // namespace mavsdk diff --git a/src/mavsdk/plugins/action/action_impl.cpp b/src/mavsdk/plugins/action/action_impl.cpp index bfc6a3608d..3ccc23dc50 100644 --- a/src/mavsdk/plugins/action/action_impl.cpp +++ b/src/mavsdk/plugins/action/action_impl.cpp @@ -257,7 +257,7 @@ void ActionImpl::arm_async(const Action::ResultCallback& callback) const bool ActionImpl::need_hold_before_arm() const { - if (_parent->autopilot() == SystemImpl::Autopilot::Px4) { + if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) { return need_hold_before_arm_px4(); } else { return need_hold_before_arm_apm(); @@ -335,8 +335,8 @@ void ActionImpl::reboot_async(const Action::ResultCallback& callback) const command.command = MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN; command.params.maybe_param1 = 1.0f; // reboot autopilot command.params.maybe_param2 = 1.0f; // reboot onboard computer - command.params.maybe_param3 = 1.0f; // reboot camera - command.params.maybe_param4 = 1.0f; // reboot gimbal + command.params.maybe_param3 = 1.0f; // reboot component + command.params.maybe_param4 = 0.0f; // all components command.target_component_id = _parent->get_autopilot_id(); _parent->send_command_async( @@ -352,8 +352,8 @@ void ActionImpl::shutdown_async(const Action::ResultCallback& callback) const command.command = MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN; command.params.maybe_param1 = 2.0f; // shutdown autopilot command.params.maybe_param2 = 2.0f; // shutdown onboard computer - command.params.maybe_param3 = 2.0f; // shutdown camera - command.params.maybe_param4 = 2.0f; // shutdown gimbal + command.params.maybe_param3 = 2.0f; // shutdown component + command.params.maybe_param4 = 0.0f; // all components command.target_component_id = _parent->get_autopilot_id(); _parent->send_command_async( @@ -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); @@ -378,6 +378,8 @@ void ActionImpl::takeoff_async_px4(const Action::ResultCallback& callback) const command.command = MAV_CMD_NAV_TAKEOFF; command.target_component_id = _parent->get_autopilot_id(); + command.params.maybe_param7 = get_takeoff_altitude().second; + _parent->send_command_async( command, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); @@ -421,6 +423,8 @@ void ActionImpl::land_async(const Action::ResultCallback& callback) const command.params.maybe_param4 = NAN; // Don't change yaw. command.target_component_id = _parent->get_autopilot_id(); + // We don't know the altitude of the landing spot, so we don't set it. + _parent->send_command_async( command, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); @@ -448,6 +452,7 @@ void ActionImpl::goto_location_async( command.command = MAV_CMD_DO_REPOSITION; command.target_component_id = _parent->get_autopilot_id(); + command.params.maybe_param2 = static_cast(MAV_DO_REPOSITION_FLAGS_CHANGE_MODE); command.params.maybe_param4 = static_cast(to_rad_from_deg(yaw_deg)); command.params.x = int32_t(std::round(latitude_deg * 1e7)); command.params.y = int32_t(std::round(longitude_deg * 1e7)); @@ -459,19 +464,21 @@ void ActionImpl::goto_location_async( }); }; - // Change to Hold mode first - if (_parent->get_flight_mode() != FlightMode::Hold) { - _parent->set_flight_mode_async( - 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) { - command_result_callback(result, callback); - return; - } - send_do_reposition(); - }); - return; + if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) { + // Change to Hold mode first + if (_parent->get_flight_mode() != FlightMode::Hold) { + _parent->set_flight_mode_async( + 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) { + command_result_callback(result, callback); + return; + } + send_do_reposition(); + }); + return; + } } send_do_reposition(); @@ -486,8 +493,12 @@ void ActionImpl::do_orbit_async( const double absolute_altitude_m, const Action::ResultCallback& callback) { - MavlinkCommandSender::CommandInt command{}; + // The DO_ORBIT command is marked as WIP, hence only supported for PX4 for now. + if (_parent->compatibility_mode() != System::CompatibilityMode::Px4) { + _parent->call_user_callback([callback]() { callback(Action::Result::Unsupported); }); + } + MavlinkCommandSender::CommandInt command{}; command.command = MAV_CMD_DO_ORBIT; command.target_component_id = _parent->get_autopilot_id(); command.params.maybe_param1 = radius_m; @@ -514,6 +525,7 @@ void ActionImpl::hold_async(const Action::ResultCallback& callback) const void ActionImpl::set_actuator_async( const int index, const float value, const Action::ResultCallback& callback) { + // TODO: improve the API to accept multiple actuators. MavlinkCommandSender::CommandLong command{}; command.command = MAV_CMD_DO_SET_ACTUATOR; @@ -551,14 +563,16 @@ void ActionImpl::transition_to_fixedwing_async(const Action::ResultCallback& cal { if (!_vtol_transition_support_known) { if (callback) { - callback(Action::Result::VtolTransitionSupportUnknown); + _parent->call_user_callback( + [callback]() { callback(Action::Result::VtolTransitionSupportUnknown); }); } return; } if (!_vtol_transition_possible) { if (callback) { - callback(Action::Result::NoVtolTransitionSupport); + _parent->call_user_callback( + [callback]() { callback(Action::Result::NoVtolTransitionSupport); }); } return; } @@ -618,26 +632,37 @@ void ActionImpl::process_extended_sys_state(const mavlink_message_t& message) void ActionImpl::set_takeoff_altitude_async( const float relative_altitude_m, const Action::ResultCallback& callback) { - callback(set_takeoff_altitude(relative_altitude_m)); -} + if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) { + // For PX4 we have to access the param as of 2022-03. + // TODO: change at some point in the future + _parent->set_param_float_async( + TAKEOFF_ALT_PARAM, + relative_altitude_m, + [callback, this](MAVLinkParameters::Result result) { + _parent->call_user_callback([callback, result] { + callback( + (result == MAVLinkParameters::Result::Success) ? + Action::Result::Success : + Action::Result::ParameterError); + }); + }, + this); -Action::Result ActionImpl::set_takeoff_altitude(float relative_altitude_m) -{ - if (_parent->autopilot() == SystemImpl::Autopilot::Px4) { - return set_takeoff_altitude_px4(relative_altitude_m); } else { - return set_takeoff_altitude_apm(relative_altitude_m); + _takeoff_altitude = relative_altitude_m; + _parent->call_user_callback([callback]() { callback(Action::Result::Success); }); } } -Action::Result ActionImpl::set_takeoff_altitude_px4(float relative_altitude_m) +Action::Result ActionImpl::set_takeoff_altitude(float relative_altitude_m) { - _takeoff_altitude = relative_altitude_m; + auto prom = std::promise(); + auto fut = prom.get_future(); + + set_takeoff_altitude_async( + relative_altitude_m, [&prom](Action::Result result) { prom.set_value(result); }); - const MAVLinkParameters::Result result = - _parent->set_param_float(TAKEOFF_ALT_PARAM, relative_altitude_m); - return (result == MAVLinkParameters::Result::Success) ? Action::Result::Success : - Action::Result::ParameterError; + return fut.get(); } Action::Result ActionImpl::set_takeoff_altitude_apm(float relative_altitude_m) @@ -649,79 +674,187 @@ Action::Result ActionImpl::set_takeoff_altitude_apm(float relative_altitude_m) void ActionImpl::get_takeoff_altitude_async( const Action::GetTakeoffAltitudeCallback& callback) const { - auto altitude_result = get_takeoff_altitude(); - callback(altitude_result.first, altitude_result.second); + if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) { + // When using with PX4 we use the param which will give the correct result for now. + // That's because PX4 as of 2022-03 does not support takeoff altitude as part of + // the command param. + // TODO: change at some point in the future + _parent->get_param_float_async( + TAKEOFF_ALT_PARAM, + [callback, this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::Success) { + _parent->call_user_callback( + [callback, value]() { callback(Action::Result::Success, value); }); + } else { + _parent->call_user_callback( + [callback]() { callback(Action::Result::ParameterError, NAN); }); + } + }, + this); + } else { + _parent->call_user_callback( + [callback, this]() { callback(Action::Result::Success, _takeoff_altitude); }); + } } std::pair ActionImpl::get_takeoff_altitude() const { - if (_parent->autopilot() == SystemImpl::Autopilot::ArduPilot) { - return std::make_pair<>(Action::Result::Success, _takeoff_altitude); - } else { - auto result = _parent->get_param_float(TAKEOFF_ALT_PARAM); - return std::make_pair<>( - (result.first == MAVLinkParameters::Result::Success) ? Action::Result::Success : - Action::Result::ParameterError, - result.second); - } + auto prom = std::promise>(); + auto fut = prom.get_future(); + + get_takeoff_altitude_async([&prom](Action::Result result, float value) { + prom.set_value(std::pair(result, value)); + }); + + return fut.get(); } void ActionImpl::set_maximum_speed_async( const float speed_m_s, const Action::ResultCallback& callback) const { - callback(set_maximum_speed(speed_m_s)); + if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) { + // For PX4 we could switch over to the command instead of the param + // but that would mean that we no longer have the setter functionality. + _parent->set_param_float_async( + MAX_SPEED_PARAM, + speed_m_s, + [callback, this](MAVLinkParameters::Result result) { + _parent->call_user_callback([callback, result] { + callback( + (result == MAVLinkParameters::Result::Success) ? + Action::Result::Success : + Action::Result::ParameterError); + }); + }, + this); + + } else { + MavlinkCommandSender::CommandLong command{}; + command.command = MAV_CMD_DO_CHANGE_SPEED; + command.target_component_id = _parent->get_autopilot_id(); + command.params.maybe_param1 = 1.0f; // ground speed + command.params.maybe_param2 = speed_m_s; // ground speed + command.params.maybe_param3 = -1.0f; // no throttle change + command.params.maybe_param4 = 0.0f; // absolute + + _parent->send_command_async( + command, [this, callback](MavlinkCommandSender::Result result, float) { + command_result_callback(result, callback); + }); + } } Action::Result ActionImpl::set_maximum_speed(float speed_m_s) const { - const MAVLinkParameters::Result result = _parent->set_param_float(MAX_SPEED_PARAM, speed_m_s); - return (result == MAVLinkParameters::Result::Success) ? Action::Result::Success : - Action::Result::ParameterError; + auto prom = std::promise(); + auto fut = prom.get_future(); + + set_maximum_speed_async(speed_m_s, [&prom](Action::Result result) { prom.set_value(result); }); + + return fut.get(); } void ActionImpl::get_maximum_speed_async(const Action::GetMaximumSpeedCallback& callback) const { - auto speed_result = get_maximum_speed(); - callback(speed_result.first, speed_result.second); + if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) { + // When using with PX4 we use the param which allows us to have a getter. + _parent->get_param_float_async( + MAX_SPEED_PARAM, + [callback, this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::Success) { + _parent->call_user_callback( + [callback, value]() { callback(Action::Result::Success, value); }); + } else { + _parent->call_user_callback( + [callback]() { callback(Action::Result::ParameterError, NAN); }); + } + }, + this); + } else { + _parent->call_user_callback([callback]() { callback(Action::Result::Unsupported, NAN); }); + } } std::pair ActionImpl::get_maximum_speed() const { - auto result = _parent->get_param_float(MAX_SPEED_PARAM); - return std::make_pair<>( - (result.first == MAVLinkParameters::Result::Success) ? Action::Result::Success : - Action::Result::ParameterError, - result.second); + auto prom = std::promise>(); + auto fut = prom.get_future(); + + get_maximum_speed_async([&prom](Action::Result result, float value) { + prom.set_value(std::pair(result, value)); + }); + + return fut.get(); } void ActionImpl::set_return_to_launch_altitude_async( const float relative_altitude_m, const Action::ResultCallback& callback) const { - callback(set_return_to_launch_altitude(relative_altitude_m)); + if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) { + // For PX4 we can access a param. + _parent->set_param_float_async( + RTL_RETURN_ALTITUDE_PARAM, + relative_altitude_m, + [callback, this](MAVLinkParameters::Result result) { + _parent->call_user_callback([callback, result] { + callback( + (result == MAVLinkParameters::Result::Success) ? + Action::Result::Success : + Action::Result::ParameterError); + }); + }, + this); + + } else { + // For ArduPilot this is not clear, and also for Pure mode there is no way + // that I know of. + _parent->call_user_callback([callback]() { callback(Action::Result::Unsupported); }); + } } Action::Result ActionImpl::set_return_to_launch_altitude(const float relative_altitude_m) const { - const MAVLinkParameters::Result result = - _parent->set_param_float(RTL_RETURN_ALTITUDE_PARAM, relative_altitude_m); - return (result == MAVLinkParameters::Result::Success) ? Action::Result::Success : - Action::Result::ParameterError; + auto prom = std::promise(); + auto fut = prom.get_future(); + + set_return_to_launch_altitude_async( + relative_altitude_m, [&prom](Action::Result result) { prom.set_value(result); }); + + return fut.get(); } void ActionImpl::get_return_to_launch_altitude_async( const Action::GetReturnToLaunchAltitudeCallback& callback) const { - const auto get_result = get_return_to_launch_altitude(); - callback(get_result.first, get_result.second); + if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) { + // When using with PX4 we use the param which allows us to have a getter. + _parent->get_param_float_async( + RTL_RETURN_ALTITUDE_PARAM, + [callback, this](MAVLinkParameters::Result result, float value) { + if (result == MAVLinkParameters::Result::Success) { + _parent->call_user_callback( + [callback, value]() { callback(Action::Result::Success, value); }); + } else { + _parent->call_user_callback( + [callback]() { callback(Action::Result::ParameterError, NAN); }); + } + }, + this); + } else { + _parent->call_user_callback([callback]() { callback(Action::Result::Unsupported, NAN); }); + } } std::pair ActionImpl::get_return_to_launch_altitude() const { - auto result = _parent->get_param_float(RTL_RETURN_ALTITUDE_PARAM); - return std::make_pair<>( - (result.first == MAVLinkParameters::Result::Success) ? Action::Result::Success : - Action::Result::ParameterError, - result.second); + auto prom = std::promise>(); + auto fut = prom.get_future(); + + get_return_to_launch_altitude_async([&prom](Action::Result result, float value) { + prom.set_value(std::pair(result, value)); + }); + + return fut.get(); } void ActionImpl::set_current_speed_async(float speed_m_s, const Action::ResultCallback& callback) diff --git a/src/mavsdk/plugins/action/action_impl.h b/src/mavsdk/plugins/action/action_impl.h index 705d22b246..7c72ee6f47 100644 --- a/src/mavsdk/plugins/action/action_impl.h +++ b/src/mavsdk/plugins/action/action_impl.h @@ -121,7 +121,7 @@ class ActionImpl : public PluginImplBase { std::atomic _vtol_transition_support_known{false}; std::atomic _vtol_transition_possible{false}; - float _takeoff_altitude{2.0}; + std::atomic _takeoff_altitude{2.0}; static constexpr uint8_t VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1; static constexpr auto TAKEOFF_ALT_PARAM = "MIS_TAKEOFF_ALT"; diff --git a/src/mavsdk/plugins/calibration/calibration_impl.cpp b/src/mavsdk/plugins/calibration/calibration_impl.cpp index 43ed07d46e..42105f553e 100644 --- a/src/mavsdk/plugins/calibration/calibration_impl.cpp +++ b/src/mavsdk/plugins/calibration/calibration_impl.cpp @@ -43,6 +43,12 @@ void CalibrationImpl::disable() {} void CalibrationImpl::calibrate_gyro_async(const CalibrationCallback& callback) { + // The calibration is currently implemented using a PX4 specific + // API based on statustext for progress and instructions. + if (_parent->compatibility_mode() != System::CompatibilityMode::Px4) { + call_callback(callback, Calibration::Result::Unsupported, {}); + } + std::lock_guard lock(_calibration_mutex); if (_parent->is_armed()) { @@ -77,7 +83,7 @@ void CalibrationImpl::calibrate_gyro_async(const CalibrationCallback& callback) void CalibrationImpl::call_callback( const CalibrationCallback& callback, const Calibration::Result& result, - const Calibration::ProgressData progress_data) + const Calibration::ProgressData& progress_data) { if (callback) { _parent->call_user_callback( @@ -87,6 +93,12 @@ void CalibrationImpl::call_callback( void CalibrationImpl::calibrate_accelerometer_async(const CalibrationCallback& callback) { + // The calibration is currently implemented using a PX4 specific + // API based on statustext for progress and instructions. + if (_parent->compatibility_mode() != System::CompatibilityMode::Px4) { + call_callback(callback, Calibration::Result::Unsupported, {}); + } + std::lock_guard lock(_calibration_mutex); if (_parent->is_armed()) { @@ -120,6 +132,12 @@ void CalibrationImpl::calibrate_accelerometer_async(const CalibrationCallback& c void CalibrationImpl::calibrate_magnetometer_async(const CalibrationCallback& callback) { + // The calibration is currently implemented using a PX4 specific + // API based on statustext for progress and instructions. + if (_parent->compatibility_mode() != System::CompatibilityMode::Px4) { + call_callback(callback, Calibration::Result::Unsupported, {}); + } + std::lock_guard lock(_calibration_mutex); if (_parent->is_armed()) { @@ -153,6 +171,12 @@ void CalibrationImpl::calibrate_magnetometer_async(const CalibrationCallback& ca void CalibrationImpl::calibrate_level_horizon_async(const CalibrationCallback& callback) { + // The calibration is currently implemented using a PX4 specific + // API based on statustext for progress and instructions. + if (_parent->compatibility_mode() != System::CompatibilityMode::Px4) { + call_callback(callback, Calibration::Result::Unsupported, {}); + } + std::lock_guard lock(_calibration_mutex); if (_parent->is_armed()) { @@ -224,6 +248,12 @@ void CalibrationImpl::calibrate_gimbal_accelerometer_async(const CalibrationCall Calibration::Result CalibrationImpl::cancel() { + // The calibration is currently implemented using a PX4 specific + // API based on statustext for progress and instructions. + if (_parent->compatibility_mode() != System::CompatibilityMode::Px4) { + return Calibration::Result::Unsupported; + } + std::lock_guard lock(_calibration_mutex); uint8_t target_component_id = MAV_COMP_ID_AUTOPILOT1; diff --git a/src/mavsdk/plugins/calibration/calibration_impl.h b/src/mavsdk/plugins/calibration/calibration_impl.h index fc312792af..93f53ac2e8 100644 --- a/src/mavsdk/plugins/calibration/calibration_impl.h +++ b/src/mavsdk/plugins/calibration/calibration_impl.h @@ -36,7 +36,7 @@ class CalibrationImpl : public PluginImplBase { void call_callback( const CalibrationCallback& callback, const Calibration::Result& result, - const Calibration::ProgressData progress_data); + const Calibration::ProgressData& progress_data); void receive_statustext(const MavlinkStatustextHandler::Statustext&); @@ -47,7 +47,6 @@ class CalibrationImpl : public PluginImplBase { void report_started(); void report_done(); - void report_warning(const std::string& warning); void report_failed(const std::string& failed); void report_cancelled(); void report_progress(float progress); @@ -57,14 +56,6 @@ class CalibrationImpl : public PluginImplBase { mutable std::mutex _calibration_mutex{}; - bool _is_gyro_ok = false; - bool _is_accelerometer_ok = false; - bool _is_magnetometer_ok = false; - - std::atomic _is_gyro_running = {false}; - std::atomic _is_accelerometer_running = {false}; - std::atomic _is_magnetometer_running = {false}; - enum class State { None, GyroCalibration, diff --git a/src/mavsdk/plugins/failure/failure_impl.cpp b/src/mavsdk/plugins/failure/failure_impl.cpp index 2f6d80c456..0160310021 100644 --- a/src/mavsdk/plugins/failure/failure_impl.cpp +++ b/src/mavsdk/plugins/failure/failure_impl.cpp @@ -23,6 +23,12 @@ void FailureImpl::deinit() {} void FailureImpl::enable() { + if (_parent->compatibility_mode() != System::CompatibilityMode::Px4) { + // Probably only PX4 implements this param. + _enabled = EnabledState::Unknown; + return; + } + constexpr auto param_name = "SYS_FAILURE_EN"; _parent->get_param_int_async( diff --git a/src/mavsdk/plugins/follow_me/follow_me_impl.cpp b/src/mavsdk/plugins/follow_me/follow_me_impl.cpp index 1c6da3ce8b..8286872d5e 100644 --- a/src/mavsdk/plugins/follow_me/follow_me_impl.cpp +++ b/src/mavsdk/plugins/follow_me/follow_me_impl.cpp @@ -311,18 +311,13 @@ void FollowMeImpl::send_target_location() return; } - SteadyTimePoint now = _time.steady_time(); - // needed by http://mavlink.org/messages/common#FOLLOW_TARGET - uint64_t elapsed_msec = - static_cast(_time.elapsed_since_s(now) * 1000); // milliseconds - std::lock_guard lock(_mutex); // LogDebug() << debug_str << "Lat: " << _target_location.latitude_deg << " Lon: " << // _target_location.longitude_deg << // " Alt: " << _target_location.absolute_altitude_m; - const int32_t lat_int = int32_t(std::round(_target_location.latitude_deg * 1e7)); - const int32_t lon_int = int32_t(std::round(_target_location.longitude_deg * 1e7)); - const float alt = static_cast(_target_location.absolute_altitude_m); + const auto lat_int = static_cast(std::round(_target_location.latitude_deg * 1e7)); + const auto lon_int = static_cast(std::round(_target_location.longitude_deg * 1e7)); + const auto alt = static_cast(_target_location.absolute_altitude_m); const float pos_std_dev[] = {NAN, NAN, NAN}; const float vel[] = { @@ -339,7 +334,7 @@ void FollowMeImpl::send_target_location() _parent->get_own_system_id(), _parent->get_own_component_id(), &msg, - elapsed_msec, + _time.elapsed_ms(), _estimation_capabilities, lat_int, lon_int, diff --git a/src/mavsdk/plugins/info/info_impl.cpp b/src/mavsdk/plugins/info/info_impl.cpp index 45627644b3..09b493ccf4 100644 --- a/src/mavsdk/plugins/info/info_impl.cpp +++ b/src/mavsdk/plugins/info/info_impl.cpp @@ -1,5 +1,4 @@ #include -#include #include #include "info_impl.h" #include "system.h" 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 fb16ad7ec9..7362cde1ad 100644 --- a/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp +++ b/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp @@ -419,7 +419,7 @@ void MissionRawImpl::clear_mission_async(const MissionRaw::ResultCallback& callb reset_mission_progress(); // For ArduPilot to clear a mission we need to upload an empty mission. - if (_parent->autopilot() == SystemImpl::Autopilot::ArduPilot) { + if (_parent->compatibility_mode() == System::CompatibilityMode::Ardupilot) { std::vector mission_items{empty_item}; upload_mission_async(mission_items, callback); } else { @@ -517,6 +517,14 @@ MissionRaw::MissionProgress MissionRawImpl::mission_progress() MissionRaw::MissionChangedHandle MissionRawImpl::subscribe_mission_changed(const MissionRaw::MissionChangedCallback& callback) { + // Listening for MISSION_ACKs can work, however, it is not guaranteed that these acks + // 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"; + // 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); return _mission_changed.callbacks.subscribe(callback); } @@ -540,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) diff --git a/src/mavsdk/plugins/offboard/offboard_impl.cpp b/src/mavsdk/plugins/offboard/offboard_impl.cpp index 710aceb6c8..653582ec00 100644 --- a/src/mavsdk/plugins/offboard/offboard_impl.cpp +++ b/src/mavsdk/plugins/offboard/offboard_impl.cpp @@ -375,14 +375,6 @@ Offboard::Result OffboardImpl::set_actuator_control(Offboard::ActuatorControl ac Offboard::Result OffboardImpl::send_position_ned() { - const static uint16_t IGNORE_VX = (1 << 3); - const static uint16_t IGNORE_VY = (1 << 4); - const static uint16_t IGNORE_VZ = (1 << 5); - const static uint16_t IGNORE_AX = (1 << 6); - const static uint16_t IGNORE_AY = (1 << 7); - const static uint16_t IGNORE_AZ = (1 << 8); - const static uint16_t IGNORE_YAW_RATE = (1 << 11); - const auto position_ned_yaw = [this]() { std::lock_guard lock(_mutex); return _position_ned_yaw; @@ -397,7 +389,10 @@ Offboard::Result OffboardImpl::send_position_ned() _parent->get_system_id(), _parent->get_autopilot_id(), MAV_FRAME_LOCAL_NED, - IGNORE_VX | IGNORE_VY | IGNORE_VZ | IGNORE_AX | IGNORE_AY | IGNORE_AZ | IGNORE_YAW_RATE, + POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE | POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, position_ned_yaw.north_m, position_ned_yaw.east_m, position_ned_yaw.down_m, @@ -415,14 +410,6 @@ Offboard::Result OffboardImpl::send_position_ned() Offboard::Result OffboardImpl::send_position_global() { - const static uint16_t IGNORE_VX = (1 << 3); - const static uint16_t IGNORE_VY = (1 << 4); - const static uint16_t IGNORE_VZ = (1 << 5); - const static uint16_t IGNORE_AX = (1 << 6); - const static uint16_t IGNORE_AY = (1 << 7); - const static uint16_t IGNORE_AZ = (1 << 8); - const static uint16_t IGNORE_YAW_RATE = (1 << 11); - const auto position_global_yaw = [this]() { std::lock_guard lock(_mutex); return _position_global_yaw; @@ -453,7 +440,10 @@ Offboard::Result OffboardImpl::send_position_global() _parent->get_system_id(), _parent->get_autopilot_id(), frame, - IGNORE_VX | IGNORE_VY | IGNORE_VZ | IGNORE_AX | IGNORE_AY | IGNORE_AZ | IGNORE_YAW_RATE, + POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | + POSITION_TARGET_TYPEMASK_VZ_IGNORE | POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, (int32_t)(position_global_yaw.lat_deg * 1.0e7), (int32_t)(position_global_yaw.lon_deg * 1.0e7), position_global_yaw.alt_m, @@ -471,14 +461,6 @@ Offboard::Result OffboardImpl::send_position_global() Offboard::Result OffboardImpl::send_velocity_ned() { - const static uint16_t IGNORE_X = (1 << 0); - const static uint16_t IGNORE_Y = (1 << 1); - const static uint16_t IGNORE_Z = (1 << 2); - const static uint16_t IGNORE_AX = (1 << 6); - const static uint16_t IGNORE_AY = (1 << 7); - const static uint16_t IGNORE_AZ = (1 << 8); - const static uint16_t IGNORE_YAW_RATE = (1 << 11); - const auto velocity_ned_yaw = [this]() { std::lock_guard lock(_mutex); return _velocity_ned_yaw; @@ -493,7 +475,10 @@ Offboard::Result OffboardImpl::send_velocity_ned() _parent->get_system_id(), _parent->get_autopilot_id(), MAV_FRAME_LOCAL_NED, - IGNORE_X | IGNORE_Y | IGNORE_Z | IGNORE_AX | IGNORE_AY | IGNORE_AZ | IGNORE_YAW_RATE, + POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_Z_IGNORE | POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, 0.0f, // x, 0.0f, // y, 0.0f, // z, @@ -511,11 +496,6 @@ Offboard::Result OffboardImpl::send_velocity_ned() Offboard::Result OffboardImpl::send_position_velocity_ned() { - const static uint16_t IGNORE_AX = (1 << 6); - const static uint16_t IGNORE_AY = (1 << 7); - const static uint16_t IGNORE_AZ = (1 << 8); - const static uint16_t IGNORE_YAW_RATE = (1 << 11); - const auto position_and_velocity = [this]() { std::lock_guard lock(_mutex); return std::make_pair<>(_position_ned_yaw, _velocity_ned_yaw); @@ -530,7 +510,8 @@ Offboard::Result OffboardImpl::send_position_velocity_ned() _parent->get_system_id(), _parent->get_autopilot_id(), MAV_FRAME_LOCAL_NED, - IGNORE_AX | IGNORE_AY | IGNORE_AZ | IGNORE_YAW_RATE, + POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | + POSITION_TARGET_TYPEMASK_AZ_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, position_and_velocity.first.north_m, position_and_velocity.first.east_m, position_and_velocity.first.down_m, @@ -548,15 +529,6 @@ Offboard::Result OffboardImpl::send_position_velocity_ned() Offboard::Result OffboardImpl::send_acceleration_ned() { - const static uint16_t IGNORE_X = (1 << 0); - const static uint16_t IGNORE_Y = (1 << 1); - const static uint16_t IGNORE_Z = (1 << 2); - const static uint16_t IGNORE_VX = (1 << 3); - const static uint16_t IGNORE_VY = (1 << 4); - const static uint16_t IGNORE_VZ = (1 << 5); - const static uint16_t IGNORE_YAW = (1 << 10); - const static uint16_t IGNORE_YAW_RATE = (1 << 11); - const auto acceleration_ned = [this]() { std::lock_guard lock(_mutex); return _acceleration_ned; @@ -571,8 +543,10 @@ Offboard::Result OffboardImpl::send_acceleration_ned() _parent->get_system_id(), _parent->get_autopilot_id(), MAV_FRAME_LOCAL_NED, - IGNORE_X | IGNORE_Y | IGNORE_Z | IGNORE_VX | IGNORE_VY | IGNORE_VZ | IGNORE_YAW | - IGNORE_YAW_RATE, + POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_Z_IGNORE | POSITION_TARGET_TYPEMASK_VX_IGNORE | + POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE, 0.0f, // x, 0.0f, // y, 0.0f, // z, @@ -590,14 +564,6 @@ Offboard::Result OffboardImpl::send_acceleration_ned() Offboard::Result OffboardImpl::send_velocity_body() { - const static uint16_t IGNORE_X = (1 << 0); - const static uint16_t IGNORE_Y = (1 << 1); - const static uint16_t IGNORE_Z = (1 << 2); - const static uint16_t IGNORE_AX = (1 << 6); - const static uint16_t IGNORE_AY = (1 << 7); - const static uint16_t IGNORE_AZ = (1 << 8); - const static uint16_t IGNORE_YAW = (1 << 10); - const auto velocity_body_yawspeed = [this]() { std::lock_guard lock(_mutex); return _velocity_body_yawspeed; @@ -612,7 +578,10 @@ Offboard::Result OffboardImpl::send_velocity_body() _parent->get_system_id(), _parent->get_autopilot_id(), MAV_FRAME_BODY_NED, - IGNORE_X | IGNORE_Y | IGNORE_Z | IGNORE_AX | IGNORE_AY | IGNORE_AZ | IGNORE_YAW, + POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | + POSITION_TARGET_TYPEMASK_Z_IGNORE | POSITION_TARGET_TYPEMASK_AX_IGNORE | + POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | + POSITION_TARGET_TYPEMASK_YAW_IGNORE, 0.0f, // x 0.0f, // y 0.0f, // z @@ -630,10 +599,6 @@ Offboard::Result OffboardImpl::send_velocity_body() Offboard::Result OffboardImpl::send_attitude() { - const static uint8_t IGNORE_BODY_ROLL_RATE = (1 << 0); - const static uint8_t IGNORE_BODY_PITCH_RATE = (1 << 1); - const static uint8_t IGNORE_BODY_YAW_RATE = (1 << 2); - const auto attitude = [this]() { std::lock_guard lock(_mutex); return _attitude; @@ -668,7 +633,9 @@ Offboard::Result OffboardImpl::send_attitude() static_cast(_parent->get_time().elapsed_ms()), _parent->get_system_id(), _parent->get_autopilot_id(), - IGNORE_BODY_ROLL_RATE | IGNORE_BODY_PITCH_RATE | IGNORE_BODY_YAW_RATE, + ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE | + ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE | + ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE, q, 0, 0, @@ -681,8 +648,6 @@ Offboard::Result OffboardImpl::send_attitude() Offboard::Result OffboardImpl::send_attitude_rate() { - const static uint8_t IGNORE_ATTITUDE = (1 << 7); - const auto attitude_rate = [this]() { std::lock_guard lock(_mutex); return _attitude_rate; @@ -698,7 +663,7 @@ Offboard::Result OffboardImpl::send_attitude_rate() static_cast(_parent->get_time().elapsed_ms()), _parent->get_system_id(), _parent->get_autopilot_id(), - IGNORE_ATTITUDE, + ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE, 0, to_rad_from_deg(attitude_rate.roll_deg_s), to_rad_from_deg(attitude_rate.pitch_deg_s), diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp index a51bf3786f..48f57fbeaa 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp @@ -2670,7 +2670,7 @@ void TelemetryImpl::check_calibration() } } if (_parent->has_autopilot()) { - if (_parent->autopilot() == SystemImpl::Autopilot::ArduPilot) { + if (_parent->compatibility_mode() == System::CompatibilityMode::Ardupilot) { // We need to ask for the home position from ArduPilot request_home_position_async(); @@ -2739,7 +2739,7 @@ void TelemetryImpl::check_calibration() }, this); - } else { + } else if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) { _parent->get_param_int_async( std::string("CAL_GYRO0_ID"), [this](MAVLinkParameters::Result result, int32_t value) { @@ -2773,7 +2773,7 @@ void TelemetryImpl::check_calibration() void TelemetryImpl::process_parameter_update(const std::string& name) { - if (_parent->autopilot() == SystemImpl::Autopilot::ArduPilot) { + if (_parent->compatibility_mode() == System::CompatibilityMode::Ardupilot) { if (name.compare("INS_GYROFFS_X") == 0) { _parent->get_param_float_async( std::string("INS_GYROFFS_X"), @@ -2838,7 +2838,7 @@ void TelemetryImpl::process_parameter_update(const std::string& name) }, this); } - } else { + } else if (_parent->compatibility_mode() == System::CompatibilityMode::Px4) { if (name.compare("CAL_GYRO0_ID") == 0) { _parent->get_param_int_async( std::string("CAL_GYRO0_ID"),