diff --git a/include/mav/opinionated_protocols/ParamClient.h b/include/mav/opinionated_protocols/ParamClient.h index fe6ab68..a6a59ea 100644 --- a/include/mav/opinionated_protocols/ParamClient.h +++ b/include/mav/opinionated_protocols/ParamClient.h @@ -67,6 +67,7 @@ namespace param { mav::Message readRaw(mav::Message &message, int target_system = mav::ANY_ID, int target_component = mav::ANY_ID, int retry_count=3, int item_timeout=1000) { throwAssert(message.name() == "PARAM_REQUEST_READ", "Message must be of type PARAM_REQUEST_READ"); auto response = exchangeRetry(_connection, message, "PARAM_VALUE", target_system, target_component, retry_count, item_timeout); + throwAssert(response["param_id"].as() == message["param_id"].as(), "Parameter ID mismatch"); return response; } @@ -84,7 +85,9 @@ namespace param { throwAssert(message.name() == "PARAM_SET", "Message must be of type PARAM_SET"); auto response = exchangeRetry(_connection, message, "PARAM_VALUE", target_system, target_component, retry_count, item_timeout); throwAssert(response["param_id"].as() == message["param_id"].as(), "Parameter ID mismatch"); - throwAssert(response["param_type"].as() == message["param_type"].as(), "Parameter type mismatch"); + throwAssert(response["param_type"].as() == message["param_type"].as(), "Parameter type mismatch"); + // compare as floats, as that's the native type for both types + throwAssert(response["param_value"].as() == message["param_value"].as(), "Parameter value mismatch. Setting param failed."); return response; } diff --git a/tests/opinionated_protocols/ParamClient.cpp b/tests/opinionated_protocols/ParamClient.cpp index 3287bb0..82b2c16 100644 --- a/tests/opinionated_protocols/ParamClient.cpp +++ b/tests/opinionated_protocols/ParamClient.cpp @@ -31,4 +31,293 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ + +#include "../doctest.h" #include "mav/opinionated_protocols/ParamClient.h" +#include "mav/TCPClient.h" +#include "mav/TCPServer.h" +#include "ProtocolTestSequencer.h" + +using namespace mav; +#define PORT 13975 + + +TEST_CASE("Param client") { + MessageSet message_set; + message_set.addFromXMLString(R""""( + + + + Specifies the datatype of a MAVLink parameter. + + 8-bit unsigned integer + + + 8-bit signed integer + + + 16-bit unsigned integer + + + 16-bit signed integer + + + 32-bit unsigned integer + + + 32-bit signed integer + + + 64-bit unsigned integer + + + 64-bit signed integer + + + 32-bit floating-point + + + 64-bit floating-point + + + + + + Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + Autopilot type / class. defined in MAV_AUTOPILOT ENUM + System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h + A bitfield for use for autopilot-specific flags. + System status flag, see MAV_STATE ENUM + MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version + + + Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also https://mavlink.io/en/services/parameter.html for a full documentation of QGroundControl and IMU code. + System ID + Component ID + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) + + + Request all parameters of this component. After this request, all parameters are emitted. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html + System ID + Component ID + + + Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Onboard parameter value + Onboard parameter type. + Total number of onboard parameters + Index of this onboard parameter + + + Set a parameter value (write new value to permanent storage). + The receiving component should acknowledge the new parameter value by broadcasting a PARAM_VALUE message (broadcasting ensures that multiple GCS all have an up-to-date list of all parameters). If the sending GCS did not receive a PARAM_VALUE within its timeout time, it should re-send the PARAM_SET message. The parameter microservice is documented at https://mavlink.io/en/services/parameter.html. + PARAM_SET may also be called within the context of a transaction (started with MAV_CMD_PARAM_TRANSACTION). Within a transaction the receiving component should respond with PARAM_ACK_TRANSACTION to the setter component (instead of broadcasting PARAM_VALUE), and PARAM_SET should be re-sent if this is ACK not received. + System ID + Component ID + Onboard parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + Onboard parameter value + Onboard parameter type. + + + +)""""); + + mav::TCPServer server_physical(PORT); + mav::NetworkRuntime server_runtime({1, 1}, message_set, server_physical); + + std::promise connection_called_promise; + auto connection_called_future = connection_called_promise.get_future(); + server_runtime.onConnection([&connection_called_promise](const std::shared_ptr&) { + connection_called_promise.set_value(); + }); + + // setup client + auto heartbeat = message_set.create("HEARTBEAT")({ + {"type", 1}, + {"autopilot", 2}, + {"base_mode", 3}, + {"custom_mode", 4}, + {"system_status", 5}, + {"mavlink_version", 6}}); + + mav::TCPClient client_physical("localhost", PORT); + mav::NetworkRuntime client_runtime(message_set, heartbeat, client_physical); + + CHECK((connection_called_future.wait_for(std::chrono::seconds(2)) != std::future_status::timeout)); + + auto server_connection = server_runtime.awaitConnection(100); + server_connection->send(heartbeat); + + auto client_connection = client_runtime.awaitConnection(100); + + + SUBCASE("Can read float param") { + + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("PARAM_REQUEST_READ", [](const mav::Message &msg) { + CHECK_EQ(msg["param_index"].as(), -1); + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + }) + .out(message_set.create("PARAM_VALUE")({ + {"param_id", "DUMMY_PARAM"}, + {"param_value", 3.14f}, + {"param_type", message_set.e("MAV_PARAM_TYPE_REAL32")}, + {"param_count", 1}, + {"param_index", 0} + })); + + server_sequence.start(); + mav::param::ParamClient param_client(client_connection, message_set); + auto param = param_client.read("DUMMY_PARAM"); + server_sequence.finish(); + CHECK_EQ(std::holds_alternative(param), true); + CHECK_EQ(std::get(param), 3.14f); + } + + + SUBCASE("Can read int param") { + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("PARAM_REQUEST_READ", [](const mav::Message &msg) { + CHECK_EQ(msg["param_index"].as(), -1); + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + }) + .out(message_set.create("PARAM_VALUE")({ + {"param_id", "DUMMY_PARAM"}, + {"param_value", mav::floatPack(42)}, + {"param_type", message_set.e("MAV_PARAM_TYPE_INT32")}, + {"param_count", 1}, + {"param_index", 0} + })); + + server_sequence.start(); + mav::param::ParamClient param_client(client_connection, message_set); + auto param = param_client.read("DUMMY_PARAM"); + server_sequence.finish(); + CHECK_EQ(std::holds_alternative(param), true); + CHECK_EQ(std::get(param), 42); + } + + + SUBCASE("Throws on receiving wrong param") { + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("PARAM_REQUEST_READ", [](const mav::Message &msg) { + CHECK_EQ(msg["param_index"].as(), -1); + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + }) + .out(message_set.create("PARAM_VALUE")({ + {"param_id", "WRONG_PARAM"}, + {"param_value", 3.14f}, + {"param_type", message_set.e("MAV_PARAM_TYPE_REAL32")}, + {"param_count", 1}, + {"param_index", 0} + })); + + server_sequence.start(); + mav::param::ParamClient param_client(client_connection, message_set); + CHECK_THROWS_AS(param_client.read("DUMMY_PARAM"), mav::ProtocolException); + server_sequence.finish(); + } + + SUBCASE("Read tries 3 times to obtain the parameter") { + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("PARAM_REQUEST_READ", [](const mav::Message &msg) { + CHECK_EQ(msg["param_index"].as(), -1); + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + }) + .in("PARAM_REQUEST_READ", [](const mav::Message &msg) { + CHECK_EQ(msg["param_index"].as(), -1); + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + }) + .in("PARAM_REQUEST_READ", [](const mav::Message &msg) { + CHECK_EQ(msg["param_index"].as(), -1); + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + }) + .out(message_set.create("PARAM_VALUE")({ + {"param_id", "DUMMY_PARAM"}, + {"param_value", 3.14f}, + {"param_type", message_set.e("MAV_PARAM_TYPE_REAL32")}, + {"param_count", 1}, + {"param_index", 0} + })); + + server_sequence.start(); + mav::param::ParamClient param_client(client_connection, message_set); + auto param = param_client.read("DUMMY_PARAM"); + server_sequence.finish(); + CHECK_EQ(std::holds_alternative(param), true); + CHECK_EQ(std::get(param), 3.14f); + } + + SUBCASE("Read times out if no response") { + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("PARAM_REQUEST_READ", [](const mav::Message &msg) { + CHECK_EQ(msg["param_index"].as(), -1); + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + }); + + server_sequence.start(); + mav::param::ParamClient param_client(client_connection, message_set); + CHECK_THROWS_AS(param_client.read("DUMMY_PARAM"), mav::TimeoutException); + server_sequence.finish(); + } + + + SUBCASE("Can write float param") { + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("PARAM_SET", [&message_set](const mav::Message &msg) { + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + CHECK_EQ(msg["param_type"].as(), message_set.e("MAV_PARAM_TYPE_REAL32")); + CHECK_EQ(msg["param_value"].as(), 3.14f); + }) + .out(message_set.create("PARAM_VALUE")({ + {"param_id", "DUMMY_PARAM"}, + {"param_value", 3.14f}, + {"param_type", message_set.e("MAV_PARAM_TYPE_REAL32")}, + {"param_count", 1}, + {"param_index", 0} + })); + + server_sequence.start(); + mav::param::ParamClient param_client(client_connection, message_set); + param_client.write("DUMMY_PARAM", 3.14f); + server_sequence.finish(); + } + + SUBCASE("Can write int param") { + // mock the server response + ProtocolTestSequencer server_sequence(server_connection); + server_sequence + .in("PARAM_SET", [&message_set](const mav::Message &msg) { + CHECK_EQ(msg["param_id"].as(), "DUMMY_PARAM"); + CHECK_EQ(msg["param_type"].as(), message_set.e("MAV_PARAM_TYPE_INT32")); + CHECK_EQ(msg["param_value"].floatUnpack(), 42); + }) + .out(message_set.create("PARAM_VALUE")({ + {"param_id", "DUMMY_PARAM"}, + {"param_value", mav::floatPack(42)}, + {"param_type", message_set.e("MAV_PARAM_TYPE_INT32")}, + {"param_count", 1}, + {"param_index", 0} + })); + + server_sequence.start(); + mav::param::ParamClient param_client(client_connection, message_set); + param_client.write("DUMMY_PARAM", 42); + server_sequence.finish(); } +} + +