diff --git a/examples/params/params.cpp b/examples/params/params.cpp index 9f4c7731a..3d0522b7f 100644 --- a/examples/params/params.cpp +++ b/examples/params/params.cpp @@ -4,34 +4,120 @@ #include #include + +#include #include +#include #include #include #include +#include +#include + +std::from_chars_result our_from_chars_float(const char* first, const char* last, float& value); using namespace mavsdk; -void usage(const std::string& bin_name) -{ - std::cerr << "Usage :" << bin_name << '\n' - << "Connection URL format should be :\n" - << " For TCP : tcp://[server_host][:server_port]\n" - << " For UDP : udp://[bind_host][:bind_port]\n" - << " For Serial : serial:///path/to/serial/dev[:baudrate]\n" - << "For example, to connect to the simulator use URL: udpin://0.0.0.0:14540\n"; -} +void get_all(Param& param); +void get(Param& param, std::string name); +void set(Param& param, std::string name, std::string value); + +class CommandLineParser { +public: + static void print_usage(const std::string& bin_name) + { + std::cerr << "Usage :" << bin_name << " [args]\n" + << "\n" + << "Connection URL format should be :\n" + << " For TCP : tcp://[server_host][:server_port]\n" + << " For UDP : udp://[bind_host][:bind_port]\n" + << " For Serial : serial:///path/to/serial/dev[:baudrate]\n" + << "For example, to connect to the simulator use URL: udpin://0.0.0.0:14540\n" + << "\n" + << "Actions:\n" + << " get_all: Print all parameters\n" + << " get : Get one param\n" + << " set : Set one param\n" + << std::flush; + } + + enum class Result { + Ok, + Invalid, + }; + + enum class Action { + GetAll, + Get, + Set, + }; + + struct Args { + std::string connection; + Action action; + std::string param_name; // Empty for GetAll + std::string value; // Only used for Set + }; + + std::pair parse(int argc, char** argv) + { + if (argc < 3) { // Need at least program_name, url, and action + std::cerr << "Insufficient arguments" << std::endl; + return {Result::Invalid, {}}; + } + + Args args{}; + args.connection = argv[1]; + + std::string action_str = argv[2]; + + if (action_str == "get_all") { + if (argc != 3) { + std::cerr << "get_all takes no additional arguments" << std::endl; + return {Result::Invalid, {}}; + } + args.action = Action::GetAll; + } else if (action_str == "get") { + if (argc != 4) { + std::cerr << "get requires a parameter name" << std::endl; + return {Result::Invalid, {}}; + } + args.action = Action::Get; + args.param_name = argv[3]; + } else if (action_str == "set") { + if (argc != 5) { + std::cerr << "set requires a parameter name and value" << std::endl; + return {Result::Invalid, {}}; + } + + args.action = Action::Set; + args.param_name = argv[3]; + args.value = argv[4]; + } else { + std::cerr << "Unknown action: " << action_str << std::endl; + return {Result::Invalid, {}}; + } + return {Result::Ok, args}; + } +}; int main(int argc, char** argv) { - if (argc != 2) { - usage(argv[0]); - return 1; + CommandLineParser parser; + auto result = parser.parse(argc, argv); + switch (result.first) { + case CommandLineParser::Result::Invalid: + CommandLineParser::print_usage(argv[0]); + return 1; + + case CommandLineParser::Result::Ok: + break; } - const std::string connection_url = argv[1]; + const auto& args = result.second; Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; - const ConnectionResult connection_result = mavsdk.add_any_connection(connection_url); + const ConnectionResult connection_result = mavsdk.add_any_connection(args.connection); if (connection_result != ConnectionResult::Success) { std::cerr << "Connection failed: " << connection_result << '\n'; @@ -69,18 +155,139 @@ int main(int argc, char** argv) // Instantiate plugins. auto param = Param{system}; + switch (args.action) { + case CommandLineParser::Action::GetAll: + get_all(param); + break; + + case CommandLineParser::Action::Get: + get(param, args.param_name); + break; + + case CommandLineParser::Action::Set: + set(param, args.param_name, args.value); + break; + }; + + return 0; +} + +void get_all(Param& param) +{ // Print params once. - const auto get_all_result = param.get_all_params(); + const auto result = param.get_all_params(); std::cout << "Int params: \n"; - for (auto p : get_all_result.int_params) { + for (auto p : result.int_params) { std::cout << p.name << ": " << p.value << '\n'; } std::cout << "Float params: \n"; - for (auto p : get_all_result.float_params) { + for (auto p : result.float_params) { std::cout << p.name << ": " << p.value << '\n'; } +} - return 0; +void get(Param& param, std::string name) +{ + // This is not very pretty, but it does seem to work." + std::cerr << "Try as float param..." << std::flush; + auto result = param.get_param_float(name); + + if (result.first == Param::Result::Success) { + std::cerr << "Ok" << std::endl; + std::cout << name << ": " << result.second << '\n'; + return; + } + + if (result.first != Param::Result::WrongType) { + std::cerr << "Failed: " << result.first << std::endl; + return; + } + + std::cout << "Wrong type" << std::endl; + std::cerr << "Try as int param next..." << std::flush; + result = param.get_param_int(name); + + if (result.first == Param::Result::Success) { + std::cerr << "Ok" << std::endl; + std::cout << name << ": " << result.second << '\n'; + return; + } + + if (result.first != Param::Result::WrongType) { + std::cerr << "Failed: " << result.first << std::endl; + return; + } + + std::cerr << "Failed: " << result.first << std::endl; + std::cerr << "Neither int or float worked, should maybe try custom? (not implemented)" + << std::endl; +} + +void set(Param& param, std::string name, std::string value) +{ + // Assume integer first + int int_value; + const auto int_result = std::from_chars(value.data(), value.data() + value.size(), int_value); + if (int_result.ec == std::errc() && int_result.ptr == value.data() + value.size()) { + std::cerr << "Setting " << value << " as integer..." << std::flush; + + auto result = param.set_param_int(name, int_value); + + if (result == Param::Result::Success) { + std::cerr << "Ok" << std::endl; + return; + } + + if (result != Param::Result::WrongType) { + std::cerr << "Failed: " << result << std::endl; + return; + } + + // If we got the type wrong, we try as float next. + } + + // Try float if integer failed + float float_value; + const auto float_result = + our_from_chars_float(value.data(), value.data() + value.size(), float_value); + if (float_result.ec == std::errc() && float_result.ptr == value.data() + value.size()) { + std::cerr << "Setting " << value << " as float..." << std::flush; + + auto result = param.set_param_float(name, float_value); + + if (result == Param::Result::Success) { + std::cerr << "Ok" << std::endl; + return; + } + + if (result != Param::Result::WrongType) { + std::cerr << "Failed: " << result << std::endl; + return; + } + + std::cerr << "Failed: " << result << std::endl; + } + + std::cerr << "Neither int or float worked, should maybe try custom? (not implemented)" + << std::endl; +} + +// GCC 9/10 don't have std::from_chars for float yet, so we have to have our own (simplified) one. +std::from_chars_result our_from_chars_float(const char* first, const char* last, float& value) +{ + float parsed_value; + int chars_read; + + // sscanf returns number of successfully parsed items (should be 1) + // %n stores the number of characters read + if (sscanf(first, "%f%n", &parsed_value, &chars_read) == 1) { + if (first + chars_read <= last) { + value = parsed_value; + return {first + chars_read, std::errc{}}; + } + } + + return {first, std::errc::invalid_argument}; } diff --git a/src/mavsdk/core/mavlink_parameter_client.cpp b/src/mavsdk/core/mavlink_parameter_client.cpp index 19d78dad0..32b385b2f 100644 --- a/src/mavsdk/core/mavlink_parameter_client.cpp +++ b/src/mavsdk/core/mavlink_parameter_client.cpp @@ -669,7 +669,8 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag << ", index: " << param_value.param_index; } - if (param_value.param_index == std::numeric_limits::max()) { + if (param_value.param_index == std::numeric_limits::max() && + safe_param_id == "_HASH_CHECK") { // Ignore PX4's _HASH_CHECK param. return; } @@ -705,6 +706,18 @@ void MavlinkParameterClient::process_param_value(const mavlink_message_t& messag << ", received: " << received_value; } + if (!item.param_value.is_same_type(received_value)) { + LogErr() << "Wrong type in param set"; + _timeout_handler.remove(_timeout_cookie); + work_queue_guard->pop_front(); + if (item.callback) { + auto callback = item.callback; + work_queue_guard.reset(); + callback(MavlinkParameterClient::Result::WrongType); + } + return; + } + if (item.param_value == received_value) { // This was successful. Inform the caller. _timeout_handler.remove(_timeout_cookie);