From 92994716cec6fa1863daa3fcc9d8c02abc211032 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Mar 2022 18:56:52 +1300 Subject: [PATCH 01/20] core: add compatibility mode setter for system This adds an API to set the compatibility_mode specifically to PX4, ArduPilot, or Pure. Pure is a new mode that should be based on common without any non-standard customizations. --- src/mavsdk/core/include/mavsdk/system.h | 20 +++ src/mavsdk/core/mavlink_command_sender.cpp | 2 +- src/mavsdk/core/mavlink_mission_transfer.cpp | 2 +- src/mavsdk/core/mavlink_mission_transfer.h | 15 +- src/mavsdk/core/system.cpp | 21 +++ src/mavsdk/core/system_impl.cpp | 130 +++++++++++++++++- src/mavsdk/core/system_impl.h | 14 +- src/mavsdk/plugins/action/action_impl.cpp | 6 +- .../plugins/mission_raw/mission_raw_impl.cpp | 2 +- .../plugins/telemetry/telemetry_impl.cpp | 4 +- 10 files changed, 198 insertions(+), 18 deletions(-) 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..4d868d6cf2 100644 --- a/src/mavsdk/core/mavlink_mission_transfer.cpp +++ b/src/mavsdk/core/mavlink_mission_transfer.cpp @@ -346,7 +346,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..ed66ce24dd 100644 --- a/src/mavsdk/core/mavlink_mission_transfer.h +++ b/src/mavsdk/core/mavlink_mission_transfer.h @@ -12,11 +12,22 @@ #include "timeout_handler.h" #include "timeout_s_callback.h" #include "locked_queue.h" -#include "sender.h" +#include "mavsdk.h" namespace mavsdk { -class MavlinkMissionTransfer { +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 { Success, 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..afd6ccf868 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 @@ -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); @@ -1053,6 +1060,105 @@ 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, @@ -1306,4 +1412,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..ba732758f8 100644 --- a/src/mavsdk/plugins/action/action_impl.cpp +++ b/src/mavsdk/plugins/action/action_impl.cpp @@ -378,6 +378,10 @@ void ActionImpl::takeoff_async_px4(const Action::ResultCallback& callback) const command.command = MAV_CMD_NAV_TAKEOFF; command.target_component_id = _parent->get_autopilot_id(); + if (_parent->compatibility_mode() == System::CompatibilityMode::Ardupilot) { + command.params.maybe_param7 = get_takeoff_altitude().second; + } + _parent->send_command_async( command, [this, callback](MavlinkCommandSender::Result result, float) { command_result_callback(result, callback); @@ -655,7 +659,7 @@ void ActionImpl::get_takeoff_altitude_async( std::pair ActionImpl::get_takeoff_altitude() const { - if (_parent->autopilot() == SystemImpl::Autopilot::ArduPilot) { + if (_parent->compatibility_mode() == System::CompatibilityMode::Ardupilot) { return std::make_pair<>(Action::Result::Success, _takeoff_altitude); } else { auto result = _parent->get_param_float(TAKEOFF_ALT_PARAM); diff --git a/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp b/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp index fb16ad7ec9..c95a0aa071 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 { diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp index a51bf3786f..26a105748a 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(); @@ -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"), From cdf7df639cbcb8c0702fb9c99be2d871cb1fc944 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 8 Mar 2022 15:14:01 +1300 Subject: [PATCH 02/20] action: only switch to Hold for PX4 In Pure mode we don't require this safety precaution. --- src/mavsdk/plugins/action/action_impl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mavsdk/plugins/action/action_impl.cpp b/src/mavsdk/plugins/action/action_impl.cpp index ba732758f8..000d320e79 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(); From c431dedd197fb6370e1ca9fa973f750e3cee11ce Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 8 Mar 2022 15:16:59 +1300 Subject: [PATCH 03/20] action: update reboot and shutdown command The meaning of param4 has changed in the spec, so we have to update it. --- src/mavsdk/plugins/action/action_impl.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/mavsdk/plugins/action/action_impl.cpp b/src/mavsdk/plugins/action/action_impl.cpp index 000d320e79..ceeba6adef 100644 --- a/src/mavsdk/plugins/action/action_impl.cpp +++ b/src/mavsdk/plugins/action/action_impl.cpp @@ -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( From 8834fd8601e1edbf4d3af87c7c5cab943722e3bc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 8 Mar 2022 15:32:04 +1300 Subject: [PATCH 04/20] action: make param thread-safe (drive-by) --- src/mavsdk/plugins/action/action_impl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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"; From 0da1ef76bf4ef435f9c6c379e0ec2c69a32233ba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 8 Mar 2022 15:32:57 +1300 Subject: [PATCH 05/20] action: add note regarding landing altitude --- src/mavsdk/plugins/action/action_impl.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/mavsdk/plugins/action/action_impl.cpp b/src/mavsdk/plugins/action/action_impl.cpp index ceeba6adef..e03dfa451d 100644 --- a/src/mavsdk/plugins/action/action_impl.cpp +++ b/src/mavsdk/plugins/action/action_impl.cpp @@ -425,6 +425,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); From 304daf8ba157aa871e9aed764d5aaee58dfd50d0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 8 Mar 2022 15:33:21 +1300 Subject: [PATCH 06/20] action: add todo to improve API (drive-by) --- src/mavsdk/plugins/action/action_impl.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/mavsdk/plugins/action/action_impl.cpp b/src/mavsdk/plugins/action/action_impl.cpp index e03dfa451d..407f008691 100644 --- a/src/mavsdk/plugins/action/action_impl.cpp +++ b/src/mavsdk/plugins/action/action_impl.cpp @@ -520,6 +520,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; From 4ac541b55d73e40be2cacbc08decf3f2b16f193f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 8 Mar 2022 15:36:50 +1300 Subject: [PATCH 07/20] action: mark hack for takeoff altitude PX4 currently requires the takeoff altitude as a parameter instead of the command param. This hopefully changes in the future. This commit changes the implementation slightly and also makes sure that the hack only exists for PX4 but not for ArduPilot or Pure mode. --- src/mavsdk/plugins/action/action_impl.cpp | 217 +++++++++++++++++----- 1 file changed, 167 insertions(+), 50 deletions(-) diff --git a/src/mavsdk/plugins/action/action_impl.cpp b/src/mavsdk/plugins/action/action_impl.cpp index 407f008691..a16d78cb0a 100644 --- a/src/mavsdk/plugins/action/action_impl.cpp +++ b/src/mavsdk/plugins/action/action_impl.cpp @@ -378,9 +378,7 @@ void ActionImpl::takeoff_async_px4(const Action::ResultCallback& callback) const command.command = MAV_CMD_NAV_TAKEOFF; command.target_component_id = _parent->get_autopilot_id(); - if (_parent->compatibility_mode() == System::CompatibilityMode::Ardupilot) { - command.params.maybe_param7 = get_takeoff_altitude().second; - } + command.params.maybe_param7 = get_takeoff_altitude().second; _parent->send_command_async( command, [this, callback](MavlinkCommandSender::Result result, float) { @@ -625,26 +623,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) @@ -656,79 +665,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->compatibility_mode() == System::CompatibilityMode::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) From f369152b6537211c89fb55697a93bf30cfb43dc0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 8 Mar 2022 15:39:30 +1300 Subject: [PATCH 08/20] action: auto change mode for REPOSITION According to the spec we could switch mode automatically when sending the DO_REPOSITION command. Therefore, for ArduPilot and Pure mode, we should actually use this. For PX4, we need to leave the hack which switches to Hold first, at least for the time being. --- src/mavsdk/plugins/action/action_impl.cpp | 29 +++++++++++++---------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/src/mavsdk/plugins/action/action_impl.cpp b/src/mavsdk/plugins/action/action_impl.cpp index a16d78cb0a..9c854b7588 100644 --- a/src/mavsdk/plugins/action/action_impl.cpp +++ b/src/mavsdk/plugins/action/action_impl.cpp @@ -452,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)); @@ -463,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() != SystemImpl::FlightMode::Hold) { + _parent->set_flight_mode_async( + SystemImpl::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(); From b8190a250221521656330df756ee47a70a565f2e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 8 Mar 2022 15:41:23 +1300 Subject: [PATCH 09/20] action: only attempt ORBIT against PX4 This excludes ORBIT from ArduPilot and Pure mode for now as it is still marked WIP. We need to decide if that's the right way to go forward, or if it should still be possible to use it in these compatibilitylmodes. --- src/mavsdk/plugins/action/action_impl.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/mavsdk/plugins/action/action_impl.cpp b/src/mavsdk/plugins/action/action_impl.cpp index 9c854b7588..345fb145f7 100644 --- a/src/mavsdk/plugins/action/action_impl.cpp +++ b/src/mavsdk/plugins/action/action_impl.cpp @@ -493,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; From c4e4ecaeb22f49673e0f8bdd13647254845b6b80 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Mar 2022 08:35:52 +1300 Subject: [PATCH 10/20] action: always use callback queue (drive-by) --- src/mavsdk/plugins/action/action_impl.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/mavsdk/plugins/action/action_impl.cpp b/src/mavsdk/plugins/action/action_impl.cpp index 345fb145f7..8b6d48498d 100644 --- a/src/mavsdk/plugins/action/action_impl.cpp +++ b/src/mavsdk/plugins/action/action_impl.cpp @@ -563,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; } From 938e8550e5e3d225d1aaab2c72fc37b58d2f5fb3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Mar 2022 08:48:48 +1300 Subject: [PATCH 11/20] calibration: return unsupported if not using PX4 This is because ArduPilot would not work, and because it is not specced for Pure mode. --- .../plugins/calibration/calibration_impl.cpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/src/mavsdk/plugins/calibration/calibration_impl.cpp b/src/mavsdk/plugins/calibration/calibration_impl.cpp index 43ed07d46e..a848a9a9bf 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()) { @@ -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; From 9e70c7c11d4bbd18be48b1da16b16521f1463e1e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Mar 2022 08:49:58 +1300 Subject: [PATCH 12/20] calibration: clang-tidy and other fixes (drive-by) --- src/mavsdk/plugins/calibration/calibration_impl.cpp | 2 +- src/mavsdk/plugins/calibration/calibration_impl.h | 11 +---------- 2 files changed, 2 insertions(+), 11 deletions(-) diff --git a/src/mavsdk/plugins/calibration/calibration_impl.cpp b/src/mavsdk/plugins/calibration/calibration_impl.cpp index a848a9a9bf..42105f553e 100644 --- a/src/mavsdk/plugins/calibration/calibration_impl.cpp +++ b/src/mavsdk/plugins/calibration/calibration_impl.cpp @@ -83,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( 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, From 6c548af0abbbab8e0fd03f5498b69caf41f33019 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Mar 2022 10:39:43 +1300 Subject: [PATCH 13/20] failure: only set param for PX4 --- src/mavsdk/plugins/failure/failure_impl.cpp | 6 ++++++ 1 file changed, 6 insertions(+) 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( From 8a91a49a67b1b4a72198b85883474e8c75e1b58b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Mar 2022 10:40:14 +1300 Subject: [PATCH 14/20] follow_me: use existing ms method (drive-by) --- src/mavsdk/plugins/follow_me/follow_me_impl.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/mavsdk/plugins/follow_me/follow_me_impl.cpp b/src/mavsdk/plugins/follow_me/follow_me_impl.cpp index 1c6da3ce8b..45e4611936 100644 --- a/src/mavsdk/plugins/follow_me/follow_me_impl.cpp +++ b/src/mavsdk/plugins/follow_me/follow_me_impl.cpp @@ -311,11 +311,6 @@ 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 << @@ -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, From 92c529b62f257ed9e0094c320f4390bf80190fbe Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Mar 2022 10:41:32 +1300 Subject: [PATCH 15/20] follow_me: clang-tidy suggestions (drive-by) --- src/mavsdk/plugins/follow_me/follow_me_impl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/mavsdk/plugins/follow_me/follow_me_impl.cpp b/src/mavsdk/plugins/follow_me/follow_me_impl.cpp index 45e4611936..8286872d5e 100644 --- a/src/mavsdk/plugins/follow_me/follow_me_impl.cpp +++ b/src/mavsdk/plugins/follow_me/follow_me_impl.cpp @@ -315,9 +315,9 @@ void FollowMeImpl::send_target_location() // 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[] = { From 122cfb9ae6c5b8c53e60a1e5a25a907b6de7e86c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Mar 2022 15:51:58 +1300 Subject: [PATCH 16/20] info: remove unused include (drive-by) --- src/mavsdk/plugins/info/info_impl.cpp | 1 - 1 file changed, 1 deletion(-) 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" From dc9da9d22f327dd7d6ce77ae38e43d24b012647e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Mar 2022 15:52:14 +1300 Subject: [PATCH 17/20] mission_raw: disable mission_changed in pure mode --- src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp b/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp index c95a0aa071..bf98905ade 100644 --- a/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp +++ b/src/mavsdk/plugins/mission_raw/mission_raw_impl.cpp @@ -517,6 +517,13 @@ 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"; + return; + } + std::lock_guard lock(_mission_changed.mutex); return _mission_changed.callbacks.subscribe(callback); } From 539f9bb9d3c91ad5aa2ddf653a3b0763ab73e6cf Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 9 Mar 2022 16:58:30 +1300 Subject: [PATCH 18/20] offboard: use existing MAVLink masks (drive-by) --- src/mavsdk/plugins/offboard/offboard_impl.cpp | 87 ++++++------------- 1 file changed, 26 insertions(+), 61 deletions(-) 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), From 8ffd92ce470f2d7aa19ee878cea72a7775fe5ace Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 15 Mar 2022 13:47:40 +1300 Subject: [PATCH 19/20] telemetry: only use param fallback for PX4 --- src/mavsdk/plugins/telemetry/telemetry_impl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp index 26a105748a..48f57fbeaa 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp @@ -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) { @@ -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"), From 45c2e133d05d7152982e81d469a9cb3332a65615 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 22 Jun 2022 15:05:50 +0200 Subject: [PATCH 20/20] 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)