diff --git a/.ycm_extra_conf.py b/.ycm_extra_conf.py index 2c2589ee41..89f4da6251 100644 --- a/.ycm_extra_conf.py +++ b/.ycm_extra_conf.py @@ -87,7 +87,7 @@ '-I', 'plugins/offboard/include', '-I', -'plugins/params_raw/include', +'plugins/param/include', '-I', 'plugins/telemetry/include', '-I', diff --git a/backend/proto b/backend/proto index 222023e2f9..bcb4b65bea 160000 --- a/backend/proto +++ b/backend/proto @@ -1 +1 @@ -Subproject commit 222023e2f907dc07046c2712b88a46236be53769 +Subproject commit bcb4b65bea278da103417dec392a96beed2a2bda diff --git a/backend/src/CMakeLists.txt b/backend/src/CMakeLists.txt index 7c748efe5e..fbbfc562f6 100644 --- a/backend/src/CMakeLists.txt +++ b/backend/src/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.1) -set(COMPONENTS_LIST core action calibration gimbal camera mission telemetry info) +set(COMPONENTS_LIST core action calibration gimbal camera mission telemetry info param) include(cmake/compile_proto.cmake) @@ -43,6 +43,7 @@ target_link_libraries(backend dronecode_sdk_mission dronecode_sdk_telemetry dronecode_sdk_info + dronecode_sdk_param gRPC::grpc++ ) diff --git a/backend/src/core/core_service_impl.h b/backend/src/core/core_service_impl.h index 5d92f63d9c..4e19e3b34d 100644 --- a/backend/src/core/core_service_impl.h +++ b/backend/src/core/core_service_impl.h @@ -43,8 +43,15 @@ class CoreServiceImpl final : public dronecode_sdk::rpc::core::CoreService::Serv const rpc::core::ListRunningPluginsRequest * /* request */, dronecode_sdk::rpc::core::ListRunningPluginsResponse *response) override { - std::string plugin_names[8] = { - "action", "calibration", "gimbal", "camera", "core", "mission", "telemetry", "info"}; + std::string plugin_names[9] = {"action", + "calibration", + "camera", + "core", + "gimbal", + "info", + "mission", + "param", + "telemetry"}; for (const auto plugin_name : plugin_names) { auto plugin_info = response->add_plugin_info(); diff --git a/backend/src/grpc_server.cpp b/backend/src/grpc_server.cpp index 8c9a6deab0..6ee12cc062 100644 --- a/backend/src/grpc_server.cpp +++ b/backend/src/grpc_server.cpp @@ -21,6 +21,7 @@ void GRPCServer::run() builder.RegisterService(&_mission_service); builder.RegisterService(&_telemetry_service); builder.RegisterService(&_info_service); + builder.RegisterService(&_param_service); _server = builder.BuildAndStart(); LogInfo() << "Server started"; diff --git a/backend/src/grpc_server.h b/backend/src/grpc_server.h index 41e79e3870..72101ad936 100644 --- a/backend/src/grpc_server.h +++ b/backend/src/grpc_server.h @@ -17,6 +17,8 @@ #include "info/info_service_impl.h" #include "plugins/gimbal/gimbal.h" #include "gimbal/gimbal_service_impl.h" +#include "plugins/param/param.h" +#include "param/param_service_impl.h" namespace dronecode_sdk { namespace backend { @@ -39,7 +41,9 @@ class GRPCServer { _telemetry(_dc.system()), _telemetry_service(_telemetry), _info(_dc.system()), - _info_service(_info) + _info_service(_info), + _param(_dc.system()), + _param_service(_param) {} void run(); @@ -65,6 +69,8 @@ class GRPCServer { TelemetryServiceImpl<> _telemetry_service; Info _info; InfoServiceImpl<> _info_service; + Param _param; + ParamServiceImpl<> _param_service; std::unique_ptr _server; }; diff --git a/backend/src/plugins/param/param_service_impl.h b/backend/src/plugins/param/param_service_impl.h new file mode 100644 index 0000000000..a2fe9d4cb2 --- /dev/null +++ b/backend/src/plugins/param/param_service_impl.h @@ -0,0 +1,113 @@ +#include "plugins/param/param.h" +#include "param/param.grpc.pb.h" + +namespace dronecode_sdk { +namespace backend { + +template +class ParamServiceImpl final : public rpc::param::ParamService::Service { +public: + ParamServiceImpl(Param ¶m) : _param(param) {} + + grpc::Status GetIntParam(grpc::ServerContext * /* context */, + const rpc::param::GetIntParamRequest *request, + rpc::param::GetIntParamResponse *response) override + { + if (request != nullptr) { + const auto requested_param = request->name(); + + if (response != nullptr) { + auto result_pair = _param.get_param_int(requested_param); + + auto *rpc_param_result = new rpc::param::ParamResult(); + rpc_param_result->set_result( + static_cast(result_pair.first)); + rpc_param_result->set_result_str( + dronecode_sdk::Param::result_str(result_pair.first)); + + response->set_allocated_param_result(rpc_param_result); + response->set_value(result_pair.second); + } + } + + return grpc::Status::OK; + } + + grpc::Status SetIntParam(grpc::ServerContext * /* context */, + const rpc::param::SetIntParamRequest *request, + rpc::param::SetIntParamResponse *response) override + { + if (request != nullptr) { + const auto requested_param_name = request->name(); + const auto requested_param_value = request->value(); + + const auto param_result = + _param.set_param_int(requested_param_name, requested_param_value); + + if (response != nullptr) { + auto *rpc_param_result = new rpc::param::ParamResult(); + rpc_param_result->set_result( + static_cast(param_result)); + rpc_param_result->set_result_str(dronecode_sdk::Param::result_str(param_result)); + + response->set_allocated_param_result(rpc_param_result); + } + } + + return grpc::Status::OK; + } + + grpc::Status GetFloatParam(grpc::ServerContext * /* context */, + const rpc::param::GetFloatParamRequest *request, + rpc::param::GetFloatParamResponse *response) override + { + if (request != nullptr) { + const auto requested_param = request->name(); + + if (response != nullptr) { + auto result_pair = _param.get_param_float(requested_param); + + auto *rpc_param_result = new rpc::param::ParamResult(); + rpc_param_result->set_result( + static_cast(result_pair.first)); + rpc_param_result->set_result_str( + dronecode_sdk::Param::result_str(result_pair.first)); + + response->set_allocated_param_result(rpc_param_result); + response->set_value(result_pair.second); + } + } + + return grpc::Status::OK; + } + + grpc::Status SetFloatParam(grpc::ServerContext * /* context */, + const rpc::param::SetFloatParamRequest *request, + rpc::param::SetFloatParamResponse *response) override + { + if (request != nullptr) { + const auto requested_param_name = request->name(); + const auto requested_param_value = request->value(); + + const auto param_result = + _param.set_param_float(requested_param_name, requested_param_value); + + if (response != nullptr) { + auto *rpc_param_result = new rpc::param::ParamResult(); + rpc_param_result->set_result( + static_cast(param_result)); + rpc_param_result->set_result_str(dronecode_sdk::Param::result_str(param_result)); + + response->set_allocated_param_result(rpc_param_result); + } + } + + return grpc::Status::OK; + } + +private: + Param &_param; +}; + +} // namespace backend +} // namespace dronecode_sdk \ No newline at end of file diff --git a/integration_tests/CMakeLists.txt b/integration_tests/CMakeLists.txt index e53d4b23d0..5260201c7d 100644 --- a/integration_tests/CMakeLists.txt +++ b/integration_tests/CMakeLists.txt @@ -48,7 +48,7 @@ add_executable(integration_tests_runner mission_survey.cpp mission_raw_mission_changed.cpp offboard_velocity.cpp - params_raw.cpp + param.cpp path_checker.cpp path_checker.h system_connection_async.cpp @@ -83,7 +83,7 @@ target_link_libraries(integration_tests_runner dronecode_sdk_follow_me dronecode_sdk_camera dronecode_sdk_calibration - dronecode_sdk_params_raw + dronecode_sdk_param ${additional_libs} gtest gtest_main diff --git a/integration_tests/calibration.cpp b/integration_tests/calibration.cpp index 0d7bef1dd4..b0cbeeb808 100644 --- a/integration_tests/calibration.cpp +++ b/integration_tests/calibration.cpp @@ -4,7 +4,7 @@ #include "global_include.h" #include "dronecode_sdk.h" #include "plugins/calibration/calibration.h" -#include "plugins/params_raw/params_raw.h" +#include "plugins/param/param.h" #include "plugins/telemetry/telemetry.h" using namespace dronecode_sdk; @@ -136,8 +136,8 @@ TEST(HardwareTest, CalibrationGyroWithTelemetry) ASSERT_TRUE(system.has_autopilot()); // Reset Gyro calibration using param. - auto params_raw = std::make_shared(system); - params_raw->set_param_int("CAL_GYRO0_ID", 0); + auto param = std::make_shared(system); + param->set_param_int("CAL_GYRO0_ID", 0); // Make sure telemetry reports gyro calibration as false. auto telemetry = std::make_shared(system); diff --git a/integration_tests/param.cpp b/integration_tests/param.cpp new file mode 100644 index 0000000000..87647a561b --- /dev/null +++ b/integration_tests/param.cpp @@ -0,0 +1,119 @@ +#include +#include "integration_test_helper.h" +#include "dronecode_sdk.h" +#include "plugins/param/param.h" + +using namespace dronecode_sdk; + +TEST_F(SitlTest, ParamSad) +{ + DronecodeSDK dc; + + ConnectionResult ret = dc.add_udp_connection(); + ASSERT_EQ(ret, ConnectionResult::SUCCESS); + + // Wait for system to connect via heartbeat. + std::this_thread::sleep_for(std::chrono::seconds(2)); + + auto &system = dc.system(); + ASSERT_TRUE(system.has_autopilot()); + + auto param = std::make_shared(system); + + { + const std::pair get_result = param->get_param_float("SYS_HITL"); + EXPECT_EQ(get_result.first, Param::Result::WRONG_TYPE); + } + + { + const std::pair get_result = param->get_param_int("MPC_VEL_MANUAL"); + EXPECT_EQ(get_result.first, Param::Result::WRONG_TYPE); + } + + { + const std::pair get_result = + param->get_param_int("NAME_TOO_LOOOOONG"); + EXPECT_EQ(get_result.first, Param::Result::PARAM_NAME_TOO_LONG); + } +} + +TEST_F(SitlTest, ParamHappy) +{ + DronecodeSDK dc; + + ConnectionResult ret = dc.add_udp_connection(); + ASSERT_EQ(ret, ConnectionResult::SUCCESS); + + // Wait for system to connect via heartbeat. + std::this_thread::sleep_for(std::chrono::seconds(2)); + + auto &system = dc.system(); + ASSERT_TRUE(system.has_autopilot()); + + auto param = std::make_shared(system); + + { + const std::pair get_result1 = param->get_param_int("SYS_HITL"); + + // Get initial value. + ASSERT_EQ(get_result1.first, Param::Result::SUCCESS); + ASSERT_GE(get_result1.second, 0); + ASSERT_LE(get_result1.second, 1); + + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Toggle the value. + const Param::Result set_result1 = param->set_param_int("SYS_HITL", !get_result1.second); + EXPECT_EQ(set_result1, Param::Result::SUCCESS); + + // Verify toggle. + const std::pair get_result2 = param->get_param_int("SYS_HITL"); + EXPECT_EQ(get_result2.first, Param::Result::SUCCESS); + EXPECT_EQ(get_result2.second, !get_result1.second); + + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Reset back to initial value. + const Param::Result set_result2 = param->set_param_int("SYS_HITL", get_result1.second); + EXPECT_EQ(set_result2, Param::Result::SUCCESS); + + // Verify reset. + const std::pair get_result3 = param->get_param_int("SYS_HITL"); + EXPECT_EQ(get_result3.first, Param::Result::SUCCESS); + EXPECT_EQ(get_result3.second, get_result1.second); + } + + { + const std::pair get_result1 = + param->get_param_float("MPC_VEL_MANUAL"); + + // Get initial value. + ASSERT_EQ(get_result1.first, Param::Result::SUCCESS); + + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Toggle the value. + const Param::Result set_result1 = + param->set_param_float("MPC_VEL_MANUAL", get_result1.second + 1.0f); + EXPECT_EQ(set_result1, Param::Result::SUCCESS); + + // Verify toggle. + const std::pair get_result2 = + param->get_param_float("MPC_VEL_MANUAL"); + EXPECT_EQ(get_result2.first, Param::Result::SUCCESS); + EXPECT_FLOAT_EQ(get_result2.second, get_result1.second + 1.0f); + + std::this_thread::sleep_for(std::chrono::seconds(1)); + + // Reset back to initial value. + const Param::Result set_result2 = + param->set_param_float("MPC_VEL_MANUAL", get_result1.second); + EXPECT_EQ(set_result2, Param::Result::SUCCESS); + + // Verify reset. + const std::pair get_result3 = + param->get_param_float("MPC_VEL_MANUAL"); + EXPECT_EQ(get_result3.first, Param::Result::SUCCESS); + EXPECT_FLOAT_EQ(get_result3.second, get_result1.second); + } +} diff --git a/integration_tests/params_raw.cpp b/integration_tests/params_raw.cpp deleted file mode 100644 index f4faa8efa1..0000000000 --- a/integration_tests/params_raw.cpp +++ /dev/null @@ -1,123 +0,0 @@ -#include -#include "integration_test_helper.h" -#include "dronecode_sdk.h" -#include "plugins/params_raw/params_raw.h" - -using namespace dronecode_sdk; - -TEST_F(SitlTest, ParamsRawSad) -{ - DronecodeSDK dc; - - ConnectionResult ret = dc.add_udp_connection(); - ASSERT_EQ(ret, ConnectionResult::SUCCESS); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto &system = dc.system(); - ASSERT_TRUE(system.has_autopilot()); - - auto params_raw = std::make_shared(system); - - { - const std::pair get_result = - params_raw->get_param_float("SYS_HITL"); - EXPECT_EQ(get_result.first, ParamsRaw::Result::WRONG_TYPE); - } - - { - const std::pair get_result = - params_raw->get_param_int("MPC_VEL_MANUAL"); - EXPECT_EQ(get_result.first, ParamsRaw::Result::WRONG_TYPE); - } - - { - const std::pair get_result = - params_raw->get_param_int("NAME_TOO_LOOOOONG"); - EXPECT_EQ(get_result.first, ParamsRaw::Result::PARAM_NAME_TOO_LONG); - } -} - -TEST_F(SitlTest, ParamsRawHappy) -{ - DronecodeSDK dc; - - ConnectionResult ret = dc.add_udp_connection(); - ASSERT_EQ(ret, ConnectionResult::SUCCESS); - - // Wait for system to connect via heartbeat. - std::this_thread::sleep_for(std::chrono::seconds(2)); - - auto &system = dc.system(); - ASSERT_TRUE(system.has_autopilot()); - - auto params_raw = std::make_shared(system); - - { - const std::pair get_result1 = params_raw->get_param_int("SYS_HITL"); - - // Get initial value. - ASSERT_EQ(get_result1.first, ParamsRaw::Result::SUCCESS); - ASSERT_GE(get_result1.second, 0); - ASSERT_LE(get_result1.second, 1); - - std::this_thread::sleep_for(std::chrono::seconds(1)); - - // Toggle the value. - const ParamsRaw::Result set_result1 = - params_raw->set_param_int("SYS_HITL", !get_result1.second); - EXPECT_EQ(set_result1, ParamsRaw::Result::SUCCESS); - - // Verify toggle. - const std::pair get_result2 = params_raw->get_param_int("SYS_HITL"); - EXPECT_EQ(get_result2.first, ParamsRaw::Result::SUCCESS); - EXPECT_EQ(get_result2.second, !get_result1.second); - - std::this_thread::sleep_for(std::chrono::seconds(1)); - - // Reset back to initial value. - const ParamsRaw::Result set_result2 = - params_raw->set_param_int("SYS_HITL", get_result1.second); - EXPECT_EQ(set_result2, ParamsRaw::Result::SUCCESS); - - // Verify reset. - const std::pair get_result3 = params_raw->get_param_int("SYS_HITL"); - EXPECT_EQ(get_result3.first, ParamsRaw::Result::SUCCESS); - EXPECT_EQ(get_result3.second, get_result1.second); - } - - { - const std::pair get_result1 = - params_raw->get_param_float("MPC_VEL_MANUAL"); - - // Get initial value. - ASSERT_EQ(get_result1.first, ParamsRaw::Result::SUCCESS); - - std::this_thread::sleep_for(std::chrono::seconds(1)); - - // Toggle the value. - const ParamsRaw::Result set_result1 = - params_raw->set_param_float("MPC_VEL_MANUAL", get_result1.second + 1.0f); - EXPECT_EQ(set_result1, ParamsRaw::Result::SUCCESS); - - // Verify toggle. - const std::pair get_result2 = - params_raw->get_param_float("MPC_VEL_MANUAL"); - EXPECT_EQ(get_result2.first, ParamsRaw::Result::SUCCESS); - EXPECT_FLOAT_EQ(get_result2.second, get_result1.second + 1.0f); - - std::this_thread::sleep_for(std::chrono::seconds(1)); - - // Reset back to initial value. - const ParamsRaw::Result set_result2 = - params_raw->set_param_float("MPC_VEL_MANUAL", get_result1.second); - EXPECT_EQ(set_result2, ParamsRaw::Result::SUCCESS); - - // Verify reset. - const std::pair get_result3 = - params_raw->get_param_float("MPC_VEL_MANUAL"); - EXPECT_EQ(get_result3.first, ParamsRaw::Result::SUCCESS); - EXPECT_FLOAT_EQ(get_result3.second, get_result1.second); - } -} diff --git a/plugins/CMakeLists.txt b/plugins/CMakeLists.txt index 3f690296e9..8a108afc14 100644 --- a/plugins/CMakeLists.txt +++ b/plugins/CMakeLists.txt @@ -20,7 +20,7 @@ add_subdirectory(log_files) add_subdirectory(mission) add_subdirectory(mission_raw) add_subdirectory(offboard) -add_subdirectory(params_raw) +add_subdirectory(param) add_subdirectory(telemetry) if (ENABLE_MAVLINK_PASSTHROUGH) diff --git a/plugins/param/CMakeLists.txt b/plugins/param/CMakeLists.txt new file mode 100644 index 0000000000..d9fdc6f88d --- /dev/null +++ b/plugins/param/CMakeLists.txt @@ -0,0 +1,29 @@ +add_library(dronecode_sdk_param ${PLUGIN_LIBRARY_TYPE} + param.cpp + param_impl.cpp +) + +target_link_libraries(dronecode_sdk_param + dronecode_sdk +) + +set_target_properties(dronecode_sdk_param + PROPERTIES COMPILE_FLAGS ${warnings} +) + +install(FILES + include/plugins/param/param.h + DESTINATION ${dronecode_sdk_install_include_dir}/plugins/param +) + +install(TARGETS dronecode_sdk_param + #EXPORT dronecode_sdk-targets + DESTINATION ${dronecode_sdk_install_lib_dir} +) + +target_include_directories(dronecode_sdk_param +PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/include +PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR} +) diff --git a/plugins/params_raw/include/plugins/params_raw/params_raw.h b/plugins/param/include/plugins/param/param.h similarity index 79% rename from plugins/params_raw/include/plugins/params_raw/params_raw.h rename to plugins/param/include/plugins/param/param.h index e349892053..5171cc2878 100644 --- a/plugins/params_raw/include/plugins/params_raw/params_raw.h +++ b/plugins/param/include/plugins/param/param.h @@ -9,12 +9,12 @@ namespace dronecode_sdk { class System; -class ParamsRawImpl; +class ParamImpl; /** - * @brief The ParamsRaw class provides raw access to get and set parameters. + * @brief The Param class provides raw access to get and set parameters. */ -class ParamsRaw : public PluginBase { +class Param : public PluginBase { public: /** * @brief Constructor. Creates the plugin for a specific System. @@ -22,20 +22,20 @@ class ParamsRaw : public PluginBase { * The plugin is typically created as shown below: * * ```cpp - * auto params_raw = std::make_shared(system); + * auto param = std::make_shared(system); * ``` * * @param system The specific system associated with this plugin. */ - explicit ParamsRaw(System &system); + explicit Param(System &system); /** * @brief Destructor (internal use only). */ - ~ParamsRaw(); + ~Param(); /** - * @brief Possible results returned for params_raw requests. + * @brief Possible results returned for param requests. */ enum class Result { UNKNOWN, /**< @brief Unknown error. */ @@ -47,12 +47,12 @@ class ParamsRaw : public PluginBase { }; /** - * @brief Returns a human-readable English string for `ParamsRaw::Result`. + * @brief Returns a human-readable English string for `Param::Result`. * * @param result The enum value for which a human readable string is required. - * @return Human readable string for the `ParamsRaw::Result`. + * @return Human readable string for the `Param::Result`. */ - std::string result_str(Result result); + static std::string result_str(Result result); /** * @brief Get an int parameter. @@ -93,15 +93,15 @@ class ParamsRaw : public PluginBase { /** * @brief Copy Constructor (object is not copyable). */ - ParamsRaw(const ParamsRaw &) = delete; + Param(const Param &) = delete; /** * @brief Equality operator (object is not copyable). */ - const ParamsRaw &operator=(const ParamsRaw &) = delete; + const Param &operator=(const Param &) = delete; private: /** @private Underlying implementation, set at instantiation */ - std::unique_ptr _impl; + std::unique_ptr _impl; }; } // namespace dronecode_sdk diff --git a/plugins/param/param.cpp b/plugins/param/param.cpp new file mode 100644 index 0000000000..7eb40303d6 --- /dev/null +++ b/plugins/param/param.cpp @@ -0,0 +1,50 @@ +#include "plugins/param/param.h" +#include "param_impl.h" + +namespace dronecode_sdk { + +Param::Param(System &system) : PluginBase(), _impl{new ParamImpl(system)} {} + +Param::~Param() {} + +std::pair Param::get_param_int(const std::string &name) +{ + return _impl->get_param_int(name); +} + +Param::Result Param::set_param_int(const std::string &name, int32_t value) +{ + return _impl->set_param_int(name, value); +} + +std::pair Param::get_param_float(const std::string &name) +{ + return _impl->get_param_float(name); +} + +Param::Result Param::set_param_float(const std::string &name, float value) +{ + return _impl->set_param_float(name, value); +} + +std::string Param::result_str(Result result) +{ + switch (result) { + case Param::Result::SUCCESS: + return "Success"; + case Param::Result::TIMEOUT: + return "Timeout"; + case Param::Result::CONNECTION_ERROR: + return "Connection error"; + case Param::Result::WRONG_TYPE: + return "Wrong type"; + case Param::Result::PARAM_NAME_TOO_LONG: + return "Param name too long"; + default: + // FALLTHROUGH + case Param::Result::UNKNOWN: + return "Unknown"; + } +} + +} // namespace dronecode_sdk diff --git a/plugins/params_raw/params_raw_impl.cpp b/plugins/param/param_impl.cpp similarity index 54% rename from plugins/params_raw/params_raw_impl.cpp rename to plugins/param/param_impl.cpp index 648ef80363..16ff1e116e 100644 --- a/plugins/params_raw/params_raw_impl.cpp +++ b/plugins/param/param_impl.cpp @@ -1,69 +1,68 @@ #include -#include "params_raw_impl.h" +#include "param_impl.h" #include "system.h" #include "global_include.h" namespace dronecode_sdk { -ParamsRawImpl::ParamsRawImpl(System &system) : PluginImplBase(system) +ParamImpl::ParamImpl(System &system) : PluginImplBase(system) { _parent->register_plugin(this); } -ParamsRawImpl::~ParamsRawImpl() +ParamImpl::~ParamImpl() { _parent->unregister_plugin(this); } -void ParamsRawImpl::init() {} +void ParamImpl::init() {} -void ParamsRawImpl::deinit() {} +void ParamImpl::deinit() {} -void ParamsRawImpl::enable() {} +void ParamImpl::enable() {} -void ParamsRawImpl::disable() {} +void ParamImpl::disable() {} -std::pair ParamsRawImpl::get_param_int(const std::string &name) +std::pair ParamImpl::get_param_int(const std::string &name) { std::pair result = _parent->get_param_int(name); return std::make_pair<>(result_from_mavlink_parameters_result(result.first), result.second); } -ParamsRaw::Result ParamsRawImpl::set_param_int(const std::string &name, int32_t value) +Param::Result ParamImpl::set_param_int(const std::string &name, int32_t value) { MAVLinkParameters::Result result = _parent->set_param_int(name, value); return result_from_mavlink_parameters_result(result); } -std::pair ParamsRawImpl::get_param_float(const std::string &name) +std::pair ParamImpl::get_param_float(const std::string &name) { std::pair result = _parent->get_param_float(name); return std::make_pair<>(result_from_mavlink_parameters_result(result.first), result.second); } -ParamsRaw::Result ParamsRawImpl::set_param_float(const std::string &name, float value) +Param::Result ParamImpl::set_param_float(const std::string &name, float value) { MAVLinkParameters::Result result = _parent->set_param_float(name, value); return result_from_mavlink_parameters_result(result); } -ParamsRaw::Result -ParamsRawImpl::result_from_mavlink_parameters_result(MAVLinkParameters::Result result) +Param::Result ParamImpl::result_from_mavlink_parameters_result(MAVLinkParameters::Result result) { switch (result) { case MAVLinkParameters::Result::SUCCESS: - return ParamsRaw::Result::SUCCESS; + return Param::Result::SUCCESS; case MAVLinkParameters::Result::TIMEOUT: - return ParamsRaw::Result::TIMEOUT; + return Param::Result::TIMEOUT; case MAVLinkParameters::Result::PARAM_NAME_TOO_LONG: - return ParamsRaw::Result::PARAM_NAME_TOO_LONG; + return Param::Result::PARAM_NAME_TOO_LONG; case MAVLinkParameters::Result::WRONG_TYPE: - return ParamsRaw::Result::WRONG_TYPE; + return Param::Result::WRONG_TYPE; case MAVLinkParameters::Result::CONNECTION_ERROR: - return ParamsRaw::Result::CONNECTION_ERROR; + return Param::Result::CONNECTION_ERROR; default: LogErr() << "Unknown param error"; - return ParamsRaw::Result::UNKNOWN; + return Param::Result::UNKNOWN; } } diff --git a/plugins/param/param_impl.h b/plugins/param/param_impl.h new file mode 100644 index 0000000000..70d028b98f --- /dev/null +++ b/plugins/param/param_impl.h @@ -0,0 +1,33 @@ +#pragma once + +#include "plugins/param/param.h" +#include "mavlink_include.h" +#include "plugin_impl_base.h" +#include + +namespace dronecode_sdk { + +class ParamImpl : public PluginImplBase { +public: + ParamImpl(System &system); + ~ParamImpl(); + + void init() override; + void deinit() override; + + void enable() override; + void disable() override; + + std::pair get_param_int(const std::string &name); + + Param::Result set_param_int(const std::string &name, int32_t value); + + std::pair get_param_float(const std::string &name); + + Param::Result set_param_float(const std::string &name, float value); + +private: + static Param::Result result_from_mavlink_parameters_result(MAVLinkParameters::Result result); +}; + +} // namespace dronecode_sdk diff --git a/plugins/params_raw/CMakeLists.txt b/plugins/params_raw/CMakeLists.txt deleted file mode 100644 index fb1c1ee9fa..0000000000 --- a/plugins/params_raw/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -add_library(dronecode_sdk_params_raw ${PLUGIN_LIBRARY_TYPE} - params_raw.cpp - params_raw_impl.cpp -) - -target_link_libraries(dronecode_sdk_params_raw - dronecode_sdk -) - -set_target_properties(dronecode_sdk_params_raw - PROPERTIES COMPILE_FLAGS ${warnings} -) - -install(FILES - include/plugins/params_raw/params_raw.h - DESTINATION ${dronecode_sdk_install_include_dir}/plugins/params_raw -) - -install(TARGETS dronecode_sdk_params_raw - #EXPORT dronecode_sdk-targets - DESTINATION ${dronecode_sdk_install_lib_dir} -) - -target_include_directories(dronecode_sdk_params_raw -PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/include -PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR} -) diff --git a/plugins/params_raw/params_raw.cpp b/plugins/params_raw/params_raw.cpp deleted file mode 100644 index a494ed0fe1..0000000000 --- a/plugins/params_raw/params_raw.cpp +++ /dev/null @@ -1,50 +0,0 @@ -#include "plugins/params_raw/params_raw.h" -#include "params_raw_impl.h" - -namespace dronecode_sdk { - -ParamsRaw::ParamsRaw(System &system) : PluginBase(), _impl{new ParamsRawImpl(system)} {} - -ParamsRaw::~ParamsRaw() {} - -std::pair ParamsRaw::get_param_int(const std::string &name) -{ - return _impl->get_param_int(name); -} - -ParamsRaw::Result ParamsRaw::set_param_int(const std::string &name, int32_t value) -{ - return _impl->set_param_int(name, value); -} - -std::pair ParamsRaw::get_param_float(const std::string &name) -{ - return _impl->get_param_float(name); -} - -ParamsRaw::Result ParamsRaw::set_param_float(const std::string &name, float value) -{ - return _impl->set_param_float(name, value); -} - -std::string ParamsRaw::result_str(Result result) -{ - switch (result) { - case ParamsRaw::Result::SUCCESS: - return "Success"; - case ParamsRaw::Result::TIMEOUT: - return "Timeout"; - case ParamsRaw::Result::CONNECTION_ERROR: - return "Connection error"; - case ParamsRaw::Result::WRONG_TYPE: - return "Wrong type"; - case ParamsRaw::Result::PARAM_NAME_TOO_LONG: - return "Param name too long"; - default: - // FALLTHROUGH - case ParamsRaw::Result::UNKNOWN: - return "Unknown"; - } -} - -} // namespace dronecode_sdk diff --git a/plugins/params_raw/params_raw_impl.h b/plugins/params_raw/params_raw_impl.h deleted file mode 100644 index 37f0869f9f..0000000000 --- a/plugins/params_raw/params_raw_impl.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once - -#include "plugins/params_raw/params_raw.h" -#include "mavlink_include.h" -#include "plugin_impl_base.h" -#include - -namespace dronecode_sdk { - -class ParamsRawImpl : public PluginImplBase { -public: - ParamsRawImpl(System &system); - ~ParamsRawImpl(); - - void init() override; - void deinit() override; - - void enable() override; - void disable() override; - - std::pair get_param_int(const std::string &name); - - ParamsRaw::Result set_param_int(const std::string &name, int32_t value); - - std::pair get_param_float(const std::string &name); - - ParamsRaw::Result set_param_float(const std::string &name, float value); - -private: - static ParamsRaw::Result - result_from_mavlink_parameters_result(MAVLinkParameters::Result result); -}; - -} // namespace dronecode_sdk