From fe237d7a5c8bab11fabd9f479ac02588b80dd9e9 Mon Sep 17 00:00:00 2001 From: Timothy Scott Date: Fri, 15 Nov 2019 14:01:02 +0100 Subject: [PATCH 1/8] Switched to custom version of MAVLink C Library with new messages --- .gitmodules | 2 +- mavlink/include/mavlink/v2.0 | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 18 ++++++++++++++++++ 3 files changed, 20 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index b0332ce19555..c087fb10a3fa 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ [submodule "mavlink/include/mavlink/v2.0"] path = mavlink/include/mavlink/v2.0 - url = https://github.com/mavlink/c_library_v2.git + url = https://github.com/ItsTimmy/c_library_v2.git branch = master [submodule "src/drivers/uavcan/libuavcan"] path = src/drivers/uavcan/libuavcan diff --git a/mavlink/include/mavlink/v2.0 b/mavlink/include/mavlink/v2.0 index a62c13922547..5192a2f7e5aa 160000 --- a/mavlink/include/mavlink/v2.0 +++ b/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit a62c13922547033d8dd0e3ef9da9edcce909a05d +Subproject commit 5192a2f7e5aa8de27c2176b024f8707b1afd801d diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 71c8849cd5aa..afd18d4e3ff0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -517,6 +517,24 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; } + } else if (cmd_mavlink.command == MAV_CMD_REQUEST_MESSAGE) { + uint16_t message_id = (uint16_t)(cmd_mavlink.param1 + 0.5f); + bool stream_found = false; + + for (const auto &stream : _mavlink->get_streams()) { + if (stream->get_id() == message_id) { + stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3, + vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7); + stream_found = true; + break; + } + } + + // TODO: Handle the case where a message is requested which could be sent, but for which there is no stream. + if (!stream_found) { + result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; + } + } else { send_ack = false; From 201abd4e0770c38bd83f43fe60854d3e3a8d8147 Mon Sep 17 00:00:00 2001 From: Timothy Scott Date: Thu, 31 Oct 2019 16:46:35 +0100 Subject: [PATCH 2/8] Added keeping track of service version --- src/modules/mavlink/mavlink_main.cpp | 42 ++++++++++++++++++++++ src/modules/mavlink/mavlink_main.h | 52 ++++++++++++++++++++++++++++ 2 files changed, 94 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 82232256a0b7..5553c1de13a3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1238,6 +1238,48 @@ Mavlink::send_protocol_version() set_proto_version(curr_proto_ver); } +void +Mavlink::send_microservice_version(const vehicle_command_s &command) +{ + uint16_t requested_service_id = (uint16_t)(command.param2 + 0.5f); + uint16_t requested_min_version = (uint16_t)(command.param3 + 0.5f); + uint16_t requested_max_version = (uint16_t)(command.param4 + 0.5f); + + auto &my_version = get_microservice_version(requested_service_id); + + mavlink_mavlink_service_version_t msg = {}; + msg.target_component = command.source_component; + msg.target_system = command.source_system; + msg.service_id = command.param2; + + if (requested_service_id != 0 && requested_min_version != 0 && requested_max_version != 0) { + if (my_version.min_version > requested_max_version || my_version.max_version < requested_min_version) { + msg.selected_version = 0; + msg.service_flags = 0; + my_version.selected_version = 0; + my_version.status = UNSUPPORTED; + + } else { + msg.selected_version = std::min(my_version.max_version, requested_max_version); + msg.service_flags = 1; + my_version.selected_version = msg.selected_version; + my_version.status = VALID; + } + } + + mavlink_msg_mavlink_service_version_send_struct(get_channel(), &msg); +} + +Mavlink::microservice_version &Mavlink::get_microservice_version(uint16_t service_id) +{ + if (service_id >= sizeof(_microservice_versions) / sizeof(_microservice_versions[0])) { + return _microservice_versions[0]; + + } else { + return _microservice_versions[service_id]; + } +} + MavlinkOrbSubscription * Mavlink::add_orb_subscription(const orb_id_t topic, int instance, bool disable_sharing) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 96c1833821b8..ef08c692e2e9 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -408,6 +408,11 @@ class Mavlink : public ModuleParams */ void send_protocol_version(); + /** + * Send the supported version of the given microservice + */ + void send_microservice_version(const vehicle_command_s &command); + List &get_streams() { return _streams; } float get_rate_mult() const { return _rate_mult; } @@ -532,6 +537,40 @@ class Mavlink : public ModuleParams static hrt_abstime &get_first_start_time() { return _first_start_time; } + enum microservice_version_status { + UNSELECTED = 0, ///< The version handshake has not yet been done for this service. + VALID, ///< The version handshake has been done, and a version was successfully selected. + UNSUPPORTED ///< The version handshake has been done, and no valid version is supported. + }; + + struct microservice_version { + /// Current status of this version (Has it been selected? Is it supported?) + microservice_version_status status; + /// Minimum version supported by this version of PX4 + uint16_t min_version; + /// Maximum version supported by this version of PX4 + uint16_t max_version; + /// Currently selected version to use for this Mavlink instance. + /// Only valid if status != UNSELECTED + uint16_t selected_version; + }; + + /** + * Get the version of a specific microservice. + * + * This means 2 different things: + * - Get the range of versions of a particular microservice supported by this current version of PX4. + * TODO: Figure out how these are defined. Probably by compile-time constants. + * - If the microservice version handshake has been done, then this function can also tell what version of + * the microservice was chosen. This version can be different for different instances of Mavlink, as the + * handshake is done separately for each instance. + * + * @param service_id The ID of the service for which to get information. + * @return Information about the service, as documented in @see Mavlink::microservice_version. This reference is + * valid for the lifetime of this object. + */ + microservice_version &get_microservice_version(uint16_t service_id); + protected: Mavlink *next{nullptr}; @@ -647,6 +686,19 @@ class Mavlink : public ModuleParams ping_statistics_s _ping_stats {}; + // TODO microservice versioning: Initialize this array. + // The index in the array is the service ID. This means the first entry in the array is not a real service. + // That is why it is intentionally instantiated to UNSUPPORTED. + static constexpr size_t MAVLINK_SERVICE_ID_MAX = 10; + microservice_version _microservice_versions[MAVLINK_SERVICE_ID_MAX] = { + { + .status = UNSUPPORTED, + .min_version = 0, + .max_version = 0, + .selected_version = 0 + } + }; + struct mavlink_message_buffer { int write_ptr; int read_ptr; From d3a28a513ceddf12e2ac723770560b24c10cc9e9 Mon Sep 17 00:00:00 2001 From: Timothy Scott Date: Fri, 1 Nov 2019 13:57:55 +0100 Subject: [PATCH 3/8] A few more tweaks --- src/modules/mavlink/mavlink_main.cpp | 6 +++++- src/modules/mavlink/mavlink_main.h | 17 ++++++++++++++++- 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5553c1de13a3..897c57c6f94c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1239,7 +1239,7 @@ Mavlink::send_protocol_version() } void -Mavlink::send_microservice_version(const vehicle_command_s &command) +Mavlink::microservice_version_handshake(const vehicle_command_s &command) { uint16_t requested_service_id = (uint16_t)(command.param2 + 0.5f); uint16_t requested_min_version = (uint16_t)(command.param3 + 0.5f); @@ -1267,6 +1267,10 @@ Mavlink::send_microservice_version(const vehicle_command_s &command) } } + PX4_INFO("Request for service version %du: My range: (%du, %du), Requested: (%du, %du), Selected %du", + requested_service_id, my_version.min_version, my_version.max_version, requested_min_version, requested_max_version, + my_version.selected_version); + mavlink_msg_mavlink_service_version_send_struct(get_channel(), &msg); } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ef08c692e2e9..2c21ed93034c 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -410,8 +410,16 @@ class Mavlink : public ModuleParams /** * Send the supported version of the given microservice + * TODO microservice versioning: Overload this function for the different message types. + * - vehicle_command_s: This is the REQUEST_MESSAGE command for one individual service version + * - mavlink_mavlink_service_version_t: This is a MAVLINK_SERVICE_VERSION message sent as a response. + * I don't need to send any response to this, as this will be the last step of the handshake. + * - (int min_version, int max_version): This is an internal function that sets the values in _microservice_versions, + * and doesn't bother with sending any messages. */ - void send_microservice_version(const vehicle_command_s &command); + void microservice_version_handshake(const vehicle_command_s &command); +// void microservice_version_handshake(const mavlink_mavlink_service_version_t &command); +// int microservice_version_handshake(uint16_t requested_min_version, uint16_t requested_max_version); List &get_streams() { return _streams; } @@ -696,6 +704,13 @@ class Mavlink : public ModuleParams .min_version = 0, .max_version = 0, .selected_version = 0 + }, + // TODO: Remove this. It is here for debugging. + { + .status = UNSELECTED, + .min_version = 1, + .max_version = 12, + .selected_version = 0 } }; From 990ea2b09e113fa47a6aaf75e2507772cd37a45c Mon Sep 17 00:00:00 2001 From: Timothy Scott Date: Fri, 15 Nov 2019 14:14:20 +0100 Subject: [PATCH 4/8] Added stream for sending all service versions --- src/modules/mavlink/mavlink_main.cpp | 5 ++ src/modules/mavlink/mavlink_messages.cpp | 104 ++++++++++++++++++++++- 2 files changed, 108 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 897c57c6f94c..1ae02738052b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1659,6 +1659,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GPS_RAW_INT", 1.0f); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("LOCAL_POSITION_NED", 1.0f); + configure_stream_local("MAVLINK_SERVICE_VERSION", 10.0f); configure_stream_local("NAMED_VALUE_FLOAT", 1.0f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.0f); configure_stream_local("OBSTACLE_DISTANCE", 1.0f); @@ -1698,6 +1699,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("HIGHRES_IMU", 50.0f); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("LOCAL_POSITION_NED", 30.0f); + configure_stream_local("MAVLINK_SERVICE_VERSION", 10.0f); configure_stream_local("NAMED_VALUE_FLOAT", 10.0f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); configure_stream_local("OBSTACLE_DISTANCE", 10.0f); @@ -1743,6 +1745,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GPS_RAW_INT", 1.0f); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("LOCAL_POSITION_NED", 30.0f); + configure_stream_local("MAVLINK_SERVICE_VERSION", 10.0f); configure_stream_local("NAMED_VALUE_FLOAT", 1.0f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f); configure_stream_local("ODOMETRY", 30.0f); @@ -1811,6 +1814,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("LOCAL_POSITION_NED", 30.0f); configure_stream_local("MANUAL_CONTROL", 5.0f); + configure_stream_local("MAVLINK_SERVICE_VERSION", 10.0f); configure_stream_local("NAMED_VALUE_FLOAT", 50.0f); configure_stream_local("NAV_CONTROLLER_OUTPUT", 10.0f); configure_stream_local("ODOMETRY", 30.0f); @@ -1843,6 +1847,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GLOBAL_POSITION_INT", 5.0f); configure_stream_local("GPS_RAW_INT", 0.5f); configure_stream_local("HOME_POSITION", 0.1f); + configure_stream_local("MAVLINK_SERVICE_VERSION", 10.0f); configure_stream_local("NAMED_VALUE_FLOAT", 1.0f); configure_stream_local("RC_CHANNELS", 0.5f); configure_stream_local("SYS_STATUS", 0.1f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 7b607724dce3..6c62d9840303 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -5126,6 +5127,106 @@ class MavlinkStreamObstacleDistance : public MavlinkStream } }; +class MavlinkStreamServiceVersion : public MavlinkStream +{ +public: + const char *get_name() const override + { + return MavlinkStreamServiceVersion::get_name_static(); + } + + static const char *get_name_static() + { + return "MAVLINK_SERVICE_VERSION"; + } + + static uint16_t get_id_static() + { + return MAVLINK_MSG_ID_MAVLINK_SERVICE_VERSION; + } + + uint16_t get_id() override + { + return get_id_static(); + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamServiceVersion(mavlink); + } + + unsigned get_size() override + { + return MAVLINK_MSG_ID_MAVLINK_SERVICE_VERSION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + + uint16_t _current_service = 0; + bool _sending_all_services = false; + int _default_interval = -1; + + static constexpr uint16_t _max_service = 10; + + /* do not allow top copying this class */ + MavlinkStreamServiceVersion(MavlinkStreamServiceVersion &) = delete; + MavlinkStreamServiceVersion &operator = (const MavlinkStreamServiceVersion &) = delete; + +protected: + explicit MavlinkStreamServiceVersion(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + bool send(const hrt_abstime t) override + { + if (_current_service > 0 && _current_service <= _max_service) { + mavlink_mavlink_service_version_t msg; + + msg.service_id = _current_service; + msg.selected_version = 1; // TODO microservice versioning + msg.service_flags = 1; // TODO microservice versioning + msg.target_system = _mavlink->get_system_id(); + msg.target_component = _mavlink->get_component_id(); + + mavlink_msg_mavlink_service_version_send_struct(_mavlink->get_channel(), &msg); + + if (_sending_all_services) { + _current_service++; + + } else { + _current_service = 0; + } + + return true; + + } else { + _default_interval = _interval; + set_interval(INT_MAX); + return false; + } + + } + + void request_message(float param1 = 0.0, float param2 = 0.0, float param3 = 0.0, float param4 = 0.0, + float param5 = 0.0, float param6 = 0.0, float param7 = 0.0) override + { + uint16_t service_id = (uint16_t)(param2 + 0.5f); + uint16_t min_version = (uint16_t)(param3 + 0.5f); + uint16_t max_version = (uint16_t)(param4 + 0.5f); + + if (service_id == 0) { + _sending_all_services = true; + _current_service = 1; + + } else { + _sending_all_services = false; + _current_service = service_id; + } + + set_interval(_default_interval); + reset_last_sent(); + } +}; + static const StreamListItem streams_list[] = { StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static), StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static), @@ -5183,7 +5284,8 @@ static const StreamListItem streams_list[] = { StreamListItem(&MavlinkStreamGroundTruth::new_instance, &MavlinkStreamGroundTruth::get_name_static, &MavlinkStreamGroundTruth::get_id_static), StreamListItem(&MavlinkStreamPing::new_instance, &MavlinkStreamPing::get_name_static, &MavlinkStreamPing::get_id_static), StreamListItem(&MavlinkStreamOrbitStatus::new_instance, &MavlinkStreamOrbitStatus::get_name_static, &MavlinkStreamOrbitStatus::get_id_static), - StreamListItem(&MavlinkStreamObstacleDistance::new_instance, &MavlinkStreamObstacleDistance::get_name_static, &MavlinkStreamObstacleDistance::get_id_static) + StreamListItem(&MavlinkStreamObstacleDistance::new_instance, &MavlinkStreamObstacleDistance::get_name_static, &MavlinkStreamObstacleDistance::get_id_static), + StreamListItem(&MavlinkStreamServiceVersion::new_instance, &MavlinkStreamServiceVersion::get_name_static, &MavlinkStreamServiceVersion::get_id_static) }; const char *get_stream_name(const uint16_t msg_id) From 0448eee45c44478c3c6d768ab73457a12417c3f6 Mon Sep 17 00:00:00 2001 From: Timothy Scott Date: Thu, 21 Nov 2019 16:51:24 +0100 Subject: [PATCH 5/8] Added compile-time constants for service versions --- src/modules/mavlink/CMakeLists.txt | 1 + src/modules/mavlink/mavlink_main.cpp | 66 +++++++------- src/modules/mavlink/mavlink_main.h | 73 ++------------- src/modules/mavlink/mavlink_messages.cpp | 25 ++++- .../mavlink/mavlink_service_versions.cpp | 77 ++++++++++++++++ .../mavlink/mavlink_service_versions.h | 91 +++++++++++++++++++ 6 files changed, 229 insertions(+), 104 deletions(-) create mode 100644 src/modules/mavlink/mavlink_service_versions.cpp create mode 100644 src/modules/mavlink/mavlink_service_versions.h diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index e9f6a6e3f6ef..b6197470c0c0 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -59,6 +59,7 @@ px4_add_module( mavlink_stream.cpp mavlink_ulog.cpp mavlink_timesync.cpp + mavlink_service_versions.cpp MODULE_CONFIG module.yaml DEPENDS diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1ae02738052b..808d70751329 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -183,6 +183,15 @@ Mavlink::Mavlink() : if (_first_start_time == 0) { _first_start_time = hrt_absolute_time(); } + + // Initialize microservice versions + for (size_t i = 0; i < microservice_versions::NUM_SERVICES; i++) { + _microservice_versions[i] = { + .metadata = µservice_versions::services_metadata[i], + .status = microservice_versions::UNKNOWN, + .selected_version = 0 + }; + } } Mavlink::~Mavlink() @@ -1238,50 +1247,39 @@ Mavlink::send_protocol_version() set_proto_version(curr_proto_ver); } -void -Mavlink::microservice_version_handshake(const vehicle_command_s &command) +microservice_versions::service_status & +Mavlink::get_service_status(uint16_t service_id) { - uint16_t requested_service_id = (uint16_t)(command.param2 + 0.5f); - uint16_t requested_min_version = (uint16_t)(command.param3 + 0.5f); - uint16_t requested_max_version = (uint16_t)(command.param4 + 0.5f); - - auto &my_version = get_microservice_version(requested_service_id); - - mavlink_mavlink_service_version_t msg = {}; - msg.target_component = command.source_component; - msg.target_system = command.source_system; - msg.service_id = command.param2; - - if (requested_service_id != 0 && requested_min_version != 0 && requested_max_version != 0) { - if (my_version.min_version > requested_max_version || my_version.max_version < requested_min_version) { - msg.selected_version = 0; - msg.service_flags = 0; - my_version.selected_version = 0; - my_version.status = UNSUPPORTED; - - } else { - msg.selected_version = std::min(my_version.max_version, requested_max_version); - msg.service_flags = 1; - my_version.selected_version = msg.selected_version; - my_version.status = VALID; + // TODO microservice versions: Maybe we don't have to search, and can just always place service ID 2 at index 2 + for (size_t i = 1; i < microservice_versions::NUM_SERVICES; i++) { + if (_microservice_versions[i].metadata->service_id == service_id) { + return _microservice_versions[i]; } } - PX4_INFO("Request for service version %du: My range: (%du, %du), Requested: (%du, %du), Selected %du", - requested_service_id, my_version.min_version, my_version.max_version, requested_min_version, requested_max_version, - my_version.selected_version); - - mavlink_msg_mavlink_service_version_send_struct(get_channel(), &msg); + return _microservice_versions[0]; } -Mavlink::microservice_version &Mavlink::get_microservice_version(uint16_t service_id) +microservice_versions::service_status & +Mavlink::determine_service_version(uint16_t service_id, uint16_t min_version, uint16_t max_version) { - if (service_id >= sizeof(_microservice_versions) / sizeof(_microservice_versions[0])) { - return _microservice_versions[0]; + microservice_versions::service_status &status = get_service_status(service_id); + + if (status.metadata->service_id == microservice_versions::MAVLINK_SERVICE_UNKNOWN) { + return status; + } + + if (status.metadata->max_version < min_version || status.metadata->min_version > max_version) { + status.status = microservice_versions::UNSUPPORTED; + status.selected_version = 0; } else { - return _microservice_versions[service_id]; + // In this branch, we know that there is overlap, so just pick the smaller of the two max versions. + status.selected_version = status.metadata->max_version < max_version ? status.metadata->max_version : max_version; + status.status = microservice_versions::SELECTED; } + + return status; } MavlinkOrbSubscription * diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 2c21ed93034c..7b6ecbb07037 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -82,6 +82,7 @@ #include "mavlink_orb_subscription.h" #include "mavlink_shell.h" #include "mavlink_ulog.h" +#include "mavlink_service_versions.h" #define DEFAULT_BAUD_RATE 57600 #define DEFAULT_DEVICE_NAME "/dev/ttyS1" @@ -408,18 +409,10 @@ class Mavlink : public ModuleParams */ void send_protocol_version(); - /** - * Send the supported version of the given microservice - * TODO microservice versioning: Overload this function for the different message types. - * - vehicle_command_s: This is the REQUEST_MESSAGE command for one individual service version - * - mavlink_mavlink_service_version_t: This is a MAVLINK_SERVICE_VERSION message sent as a response. - * I don't need to send any response to this, as this will be the last step of the handshake. - * - (int min_version, int max_version): This is an internal function that sets the values in _microservice_versions, - * and doesn't bother with sending any messages. - */ - void microservice_version_handshake(const vehicle_command_s &command); -// void microservice_version_handshake(const mavlink_mavlink_service_version_t &command); -// int microservice_version_handshake(uint16_t requested_min_version, uint16_t requested_max_version); + microservice_versions::service_status &get_service_status(uint16_t service_id); + + microservice_versions::service_status &determine_service_version(uint16_t service_id, uint16_t min_version, + uint16_t max_version); List &get_streams() { return _streams; } @@ -545,40 +538,6 @@ class Mavlink : public ModuleParams static hrt_abstime &get_first_start_time() { return _first_start_time; } - enum microservice_version_status { - UNSELECTED = 0, ///< The version handshake has not yet been done for this service. - VALID, ///< The version handshake has been done, and a version was successfully selected. - UNSUPPORTED ///< The version handshake has been done, and no valid version is supported. - }; - - struct microservice_version { - /// Current status of this version (Has it been selected? Is it supported?) - microservice_version_status status; - /// Minimum version supported by this version of PX4 - uint16_t min_version; - /// Maximum version supported by this version of PX4 - uint16_t max_version; - /// Currently selected version to use for this Mavlink instance. - /// Only valid if status != UNSELECTED - uint16_t selected_version; - }; - - /** - * Get the version of a specific microservice. - * - * This means 2 different things: - * - Get the range of versions of a particular microservice supported by this current version of PX4. - * TODO: Figure out how these are defined. Probably by compile-time constants. - * - If the microservice version handshake has been done, then this function can also tell what version of - * the microservice was chosen. This version can be different for different instances of Mavlink, as the - * handshake is done separately for each instance. - * - * @param service_id The ID of the service for which to get information. - * @return Information about the service, as documented in @see Mavlink::microservice_version. This reference is - * valid for the lifetime of this object. - */ - microservice_version &get_microservice_version(uint16_t service_id); - protected: Mavlink *next{nullptr}; @@ -694,26 +653,6 @@ class Mavlink : public ModuleParams ping_statistics_s _ping_stats {}; - // TODO microservice versioning: Initialize this array. - // The index in the array is the service ID. This means the first entry in the array is not a real service. - // That is why it is intentionally instantiated to UNSUPPORTED. - static constexpr size_t MAVLINK_SERVICE_ID_MAX = 10; - microservice_version _microservice_versions[MAVLINK_SERVICE_ID_MAX] = { - { - .status = UNSUPPORTED, - .min_version = 0, - .max_version = 0, - .selected_version = 0 - }, - // TODO: Remove this. It is here for debugging. - { - .status = UNSELECTED, - .min_version = 1, - .max_version = 12, - .selected_version = 0 - } - }; - struct mavlink_message_buffer { int write_ptr; int read_ptr; @@ -726,6 +665,8 @@ class Mavlink : public ModuleParams pthread_mutex_t _message_buffer_mutex {}; pthread_mutex_t _send_mutex {}; + microservice_versions::service_status _microservice_versions[microservice_versions::NUM_SERVICES]; + DEFINE_PARAMETERS( (ParamInt) _param_mav_sys_id, (ParamInt) _param_mav_comp_id, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6c62d9840303..15270141bbab 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -44,6 +44,7 @@ #include "mavlink_command_sender.h" #include "mavlink_simple_analyzer.h" #include "mavlink_high_latency2.h" +#include "mavlink_service_versions.h" #include #include @@ -5163,6 +5164,8 @@ class MavlinkStreamServiceVersion : public MavlinkStream private: uint16_t _current_service = 0; + uint16_t _min_version = 0; + uint16_t _max_version = 0; bool _sending_all_services = false; int _default_interval = -1; @@ -5182,11 +5185,25 @@ class MavlinkStreamServiceVersion : public MavlinkStream mavlink_mavlink_service_version_t msg; msg.service_id = _current_service; - msg.selected_version = 1; // TODO microservice versioning - msg.service_flags = 1; // TODO microservice versioning msg.target_system = _mavlink->get_system_id(); msg.target_component = _mavlink->get_component_id(); + if (_sending_all_services) { + microservice_versions::service_status &status = _mavlink->get_service_status(_current_service); + + msg.selected_version = 0; // TODO microservice versioning: How does the handshake work in "stream all services" mode? + // TODO: Add min and max version + msg.service_flags = status.metadata->min_version == 0 ? 0 : 1; + + } else { + microservice_versions::service_status &status = + _mavlink->determine_service_version(_current_service, _min_version, _max_version); + + msg.selected_version = status.selected_version; + msg.service_flags = status.metadata->min_version == 0 ? 0 : 1; + // TODO add min and max + } + mavlink_msg_mavlink_service_version_send_struct(_mavlink->get_channel(), &msg); if (_sending_all_services) { @@ -5210,8 +5227,8 @@ class MavlinkStreamServiceVersion : public MavlinkStream float param5 = 0.0, float param6 = 0.0, float param7 = 0.0) override { uint16_t service_id = (uint16_t)(param2 + 0.5f); - uint16_t min_version = (uint16_t)(param3 + 0.5f); - uint16_t max_version = (uint16_t)(param4 + 0.5f); + _min_version = (uint16_t)(param3 + 0.5f); + _max_version = (uint16_t)(param4 + 0.5f); if (service_id == 0) { _sending_all_services = true; diff --git a/src/modules/mavlink/mavlink_service_versions.cpp b/src/modules/mavlink/mavlink_service_versions.cpp new file mode 100644 index 000000000000..7c58b32f750d --- /dev/null +++ b/src/modules/mavlink/mavlink_service_versions.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_service_versions.c + * Compile-time constants for supported MAVLink microservices + * + * @author Timothy Scott + */ + +#include "mavlink_service_versions.h" + +// TODO microservice versioning: These will be replaced with the autogenerated MAVLink constants +#define MAVLINK_SERVICE_ID_MISSION 1 +#define MAVLINK_SERVICE_ID_PARAMETERS 2 + +namespace microservice_versions +{ + +const service_metadata services_metadata[NUM_SERVICES] { + { + .service_id = MAVLINK_SERVICE_UNKNOWN, + .min_version = 0, + .max_version = 0 + }, + { + .service_id = MAVLINK_SERVICE_ID_MISSION, + .min_version = 1, + .max_version = 3 + }, + { + .service_id = MAVLINK_SERVICE_ID_PARAMETERS, + .min_version = 1, + .max_version = 1 + } +}; + +// TODO microservice versioning: Remove this +// const service_metadata *get_metadata(uint16_t service_id){ +// for(size_t i = 0; i < NUM_SERVICES; i++){ +// if(services_metadata[i].service_id == service_id){ +// return &services_metadata[i]; +// } +// } +// return &services_metadata[0]; +// } +} \ No newline at end of file diff --git a/src/modules/mavlink/mavlink_service_versions.h b/src/modules/mavlink/mavlink_service_versions.h new file mode 100644 index 000000000000..1a154093982e --- /dev/null +++ b/src/modules/mavlink/mavlink_service_versions.h @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file mavlink_service_versions.h + * Compile-time constants for supported MAVLink microservices + * + * @author Timothy Scott + */ + +#pragma once + +#include + +namespace microservice_versions +{ + +constexpr size_t NUM_SERVICES = 3; +constexpr uint16_t MAVLINK_SERVICE_UNKNOWN = 0; + +/// This struct represents the information about microservice versions that is known to PX4 at compile time. +/// Specifically, it contains the minimum and maximum versions of a service supported by PX4, but NOT +/// the currently-selected version, which is negotiated at runtime, and is separate for each MAVLink instance. +struct service_metadata { + const uint16_t service_id; ///< The ID of the service + /// The minimum version of the microservice supported by this version of PX4, or 0 if this microservice + /// is not supported at all. + const uint16_t min_version; + /// The minimum version of the microservice supported by this version of PX4, or 0 if this microservice + /// is not supported at all. + const uint16_t max_version; +}; + +/** + * This enum represents the current status of the microservice version handshake for one particular microservice. + */ +enum handshake_status { + UNKNOWN, ///< The microservice version handshake has not been done for this microservice + UNSUPPORTED, ///< The handshake has been done, and there is no common version, so this service is unsupported. + SELECTED ///< The handshake has been done, and the supported version has been selected. +}; + +/** + * This struct represents the version information for one microservice, as known as runtime.. + */ +struct service_status { + /// See service_metadata. This is a pointer because there only needs to be one instance of a given + /// metadata struct, because it is immutable. + const service_metadata *metadata; + /// See service_version_status + handshake_status status; + /// If status == SELECTED, then selected_version is the version of the microservice that is currently being used. + /// If status != SELECTED, then this variable is undefined and should not be used to make any decisions. + uint16_t selected_version; +}; + +extern const service_metadata services_metadata[NUM_SERVICES]; + +// /// Returns the metadata of a given microservice. +// const service_metadata *get_metadata(uint16_t service_id); +} \ No newline at end of file From b5d86710010484ff1e8d046291bef966d459e558 Mon Sep 17 00:00:00 2001 From: Timothy Scott Date: Fri, 22 Nov 2019 16:03:07 +0100 Subject: [PATCH 6/8] Implemented microservice versioning in Mission microservice --- src/modules/mavlink/mavlink_main.cpp | 21 ++++++ src/modules/mavlink/mavlink_main.h | 29 ++++++++ src/modules/mavlink/mavlink_messages.cpp | 29 +++----- src/modules/mavlink/mavlink_mission.cpp | 72 +++++++++++-------- src/modules/mavlink/mavlink_mission.h | 2 + .../mavlink/mavlink_service_versions.cpp | 15 ++-- .../mavlink/mavlink_service_versions.h | 17 ++++- 7 files changed, 124 insertions(+), 61 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 808d70751329..03c8dd06af56 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1282,6 +1282,27 @@ Mavlink::determine_service_version(uint16_t service_id, uint16_t min_version, ui return status; } +microservice_versions::service_status & +Mavlink::determine_service_version(uint16_t service_id) +{ + microservice_versions::service_status &status = get_service_status(service_id); + + if (status.metadata->service_id == microservice_versions::MAVLINK_SERVICE_UNKNOWN) { + return status; + } + + if (status.metadata->max_version == 0) { + status.status = microservice_versions::UNSUPPORTED; + + } else { + status.status = microservice_versions::SELECTED; + } + + status.selected_version = status.metadata->max_version; + + return status; +} + MavlinkOrbSubscription * Mavlink::add_orb_subscription(const orb_id_t topic, int instance, bool disable_sharing) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 7b6ecbb07037..8e82e074446b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -409,11 +409,40 @@ class Mavlink : public ModuleParams */ void send_protocol_version(); + /** + * This function returns the status of the given service, without selecting a version to use. + * @param service_id ID of the service in question + * @return Reference to a service_status which exists for the lifetime of this Mavlink object + */ microservice_versions::service_status &get_service_status(uint16_t service_id); + /** + * Takes the min and max version supported by the communication partner, and determines the maximum version + * supported by both this version of PX4 and the communication partner. This selected version is then + * stored in this instance of Mavlink, and can be retrieved with Mavlink::get_service_status. + * + * @param service_id ID of the service being negotiated + * @param min_version Minimum version of the service supported by the other system. + * @param max_version Maximum version of the service supported by the other system. + * @return Reference to a service_status, which exists for the lifetime of this Mavlink object, and has been + * modified with the results of this service version handshake. + */ microservice_versions::service_status &determine_service_version(uint16_t service_id, uint16_t min_version, uint16_t max_version); + /** + * See Mavlink::determine_service_version(uint16_t, uint16_t, uint16_t). + * + * This overloaded function performs the same task as the other determine_service_version, but it does not know the + * min and max version supported by the other system, so it assumes that it supports every version (like a + * GCS would), and just selects the maximum version supported by this version of PX4. + * + * @param service_id ID of the service + * @return Reference to a service_status, which exists for the lifetime of this Mavlink object, and has been + * modified with the results of this service version handshake. + */ + microservice_versions::service_status &determine_service_version(uint16_t service_id); + List &get_streams() { return _streams; } float get_rate_mult() const { return _rate_mult; } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 15270141bbab..35e97625b095 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -5169,8 +5169,6 @@ class MavlinkStreamServiceVersion : public MavlinkStream bool _sending_all_services = false; int _default_interval = -1; - static constexpr uint16_t _max_service = 10; - /* do not allow top copying this class */ MavlinkStreamServiceVersion(MavlinkStreamServiceVersion &) = delete; MavlinkStreamServiceVersion &operator = (const MavlinkStreamServiceVersion &) = delete; @@ -5181,28 +5179,21 @@ class MavlinkStreamServiceVersion : public MavlinkStream bool send(const hrt_abstime t) override { - if (_current_service > 0 && _current_service <= _max_service) { + if (_current_service > 0 && _current_service < microservice_versions::NUM_SERVICES) { mavlink_mavlink_service_version_t msg; + auto &status = _sending_all_services ? _mavlink->determine_service_version(_current_service) + : _mavlink->determine_service_version(_current_service, _min_version, _max_version); + msg.service_id = _current_service; msg.target_system = _mavlink->get_system_id(); msg.target_component = _mavlink->get_component_id(); - - if (_sending_all_services) { - microservice_versions::service_status &status = _mavlink->get_service_status(_current_service); - - msg.selected_version = 0; // TODO microservice versioning: How does the handshake work in "stream all services" mode? - // TODO: Add min and max version - msg.service_flags = status.metadata->min_version == 0 ? 0 : 1; - - } else { - microservice_versions::service_status &status = - _mavlink->determine_service_version(_current_service, _min_version, _max_version); - - msg.selected_version = status.selected_version; - msg.service_flags = status.metadata->min_version == 0 ? 0 : 1; - // TODO add min and max - } + // TODO microservice versioning: How does the handshake work in "stream all services" mode? + // The current implementation is to just select my maximum version and hope for the best. + msg.selected_version = status.selected_version; + msg.service_min_version = status.metadata->min_version; + msg.service_max_version = status.metadata->max_version; + msg.service_flags = status.metadata->min_version == 0 ? 0 : 1; mavlink_msg_mavlink_service_version_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 952abf3bf595..09615bf5a5aa 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -53,6 +53,9 @@ #include #include +// TODO microservice version: Replace this with autogenerated MAVLink constant +#define MAVLINK_SERVICE_ID_MISSION 1 + using matrix::wrap_2pi; dm_item_t MavlinkMissionManager::_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -356,7 +359,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t if (read_result > 0) { _time_last_sent = hrt_absolute_time(); - if (_int_mode) { + if (int_mode()) { mavlink_mission_item_int_t wp = {}; format_mavlink_mission_item(&mission_item, reinterpret_cast(&wp)); @@ -416,13 +419,20 @@ MavlinkMissionManager::current_item_count() return _count[_mission_type]; } +bool +MavlinkMissionManager::int_mode() +{ + auto status = _mavlink->get_service_status(MAVLINK_SERVICE_ID_MISSION); + return status.selected_version > 1; +} + void MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq) { if (seq < current_max_item_count()) { _time_last_sent = hrt_absolute_time(); - if (_int_mode) { + if (int_mode()) { mavlink_mission_request_int_t wpr; wpr.target_system = sysid; wpr.target_component = compid; @@ -619,12 +629,12 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) // INT or float mode is not supported if (wpa.type == MAV_MISSION_UNSUPPORTED) { - if (_int_mode) { - _int_mode = false; + if (int_mode()) { + //int_mode() = false; send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); } else { - _int_mode = true; + //int_mode() = true; send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); } @@ -732,9 +742,9 @@ void MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) { // The request comes in the old float mode, so we switch to it. - if (_int_mode) { - _int_mode = false; - } +// if (int_mode()) { +// int_mode() = false; +// } handle_mission_request_both(msg); } @@ -743,9 +753,9 @@ void MavlinkMissionManager::handle_mission_request_int(const mavlink_message_t *msg) { // The request comes in the new int mode, so we switch to it. - if (!_int_mode) { - _int_mode = true; - } +// if (!int_mode()) { +// int_mode() = true; +// } handle_mission_request_both(msg); } @@ -968,10 +978,10 @@ MavlinkMissionManager::switch_to_idle_state() void MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) { - if (_int_mode) { - // It seems that we should be using the float mode, let's switch out of int mode. - _int_mode = false; - } +// if (int_mode()) { +// // It seems that we should be using the float mode, let's switch out of int mode. +// int_mode() = false; +// } handle_mission_item_both(msg); } @@ -979,10 +989,10 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) void MavlinkMissionManager::handle_mission_item_int(const mavlink_message_t *msg) { - if (!_int_mode) { - // It seems that we should be using the int mode, let's switch to it. - _int_mode = true; - } +// if (!int_mode()) { +// // It seems that we should be using the int mode, let's switch to it. +// int_mode() = true; +// } handle_mission_item_both(msg); } @@ -992,7 +1002,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) { // The mavlink_message could also contain a mavlink_mission_item_int_t. We ignore that here - // and take care of it later in parse_mavlink_mission_item depending on _int_mode. + // and take care of it later in parse_mavlink_mission_item depending on int_mode(). mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); @@ -1265,16 +1275,16 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * { if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL || mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || - (_int_mode && (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT || - mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT))) { + (int_mode() && (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT || + mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT))) { // Switch to int mode if that is what we are receiving - if ((mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT || - mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)) { - _int_mode = true; - } +// if ((mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT || +// mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)) { +// int_mode() = true; +// } - if (_int_mode) { + if (int_mode()) { /* The argument is actually a mavlink_mission_item_int_t in int_mode. * mavlink_mission_item_t and mavlink_mission_item_int_t have the same * alignment, so we can just swap float for int32_t. */ @@ -1543,8 +1553,8 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->param3 = 0.0f; mavlink_mission_item->param4 = 0.0f; - if (_int_mode) { - // This function actually receives a mavlink_mission_item_int_t in _int_mode + if (int_mode()) { + // This function actually receives a mavlink_mission_item_int_t in int_mode() // which has the same alignment as mavlink_mission_item_t and the only // difference is int32_t vs. float for x and y. mavlink_mission_item_int_t *item_int = @@ -1561,7 +1571,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->z = mission_item->altitude; if (mission_item->altitude_is_relative) { - if (_int_mode) { + if (int_mode()) { mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT_INT; } else { @@ -1569,7 +1579,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * } } else { - if (_int_mode) { + if (int_mode()) { mavlink_mission_item->frame = MAV_FRAME_GLOBAL_INT; } else { diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index aeaff342af74..faa4d451c006 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -153,6 +153,8 @@ class MavlinkMissionManager /** get the number of item count for the current _mission_type */ uint16_t current_item_count(); + bool int_mode(); + /* do not allow top copying this class */ MavlinkMissionManager(MavlinkMissionManager &); MavlinkMissionManager &operator = (const MavlinkMissionManager &); diff --git a/src/modules/mavlink/mavlink_service_versions.cpp b/src/modules/mavlink/mavlink_service_versions.cpp index 7c58b32f750d..0eb5b797b34b 100644 --- a/src/modules/mavlink/mavlink_service_versions.cpp +++ b/src/modules/mavlink/mavlink_service_versions.cpp @@ -43,6 +43,7 @@ // TODO microservice versioning: These will be replaced with the autogenerated MAVLink constants #define MAVLINK_SERVICE_ID_MISSION 1 #define MAVLINK_SERVICE_ID_PARAMETERS 2 +#define MAVLINK_SERVICE_ID_CAMERA 3 namespace microservice_versions { @@ -62,16 +63,12 @@ const service_metadata services_metadata[NUM_SERVICES] { .service_id = MAVLINK_SERVICE_ID_PARAMETERS, .min_version = 1, .max_version = 1 + }, + { + .service_id = MAVLINK_SERVICE_ID_CAMERA, + .min_version = 3, + .max_version = 30 } }; -// TODO microservice versioning: Remove this -// const service_metadata *get_metadata(uint16_t service_id){ -// for(size_t i = 0; i < NUM_SERVICES; i++){ -// if(services_metadata[i].service_id == service_id){ -// return &services_metadata[i]; -// } -// } -// return &services_metadata[0]; -// } } \ No newline at end of file diff --git a/src/modules/mavlink/mavlink_service_versions.h b/src/modules/mavlink/mavlink_service_versions.h index 1a154093982e..b64278145c43 100644 --- a/src/modules/mavlink/mavlink_service_versions.h +++ b/src/modules/mavlink/mavlink_service_versions.h @@ -33,7 +33,12 @@ /** * @file mavlink_service_versions.h - * Compile-time constants for supported MAVLink microservices + * Compile-time constants for supported MAVLink microservices. + * + * This file does not keep track of the status of any given microservice version handshake. Different instances of + * MAVLink can be using different versions of a microservice, so that is done inside of the Mavlink class. + * + * See Mavlink::determine_service_version(...) * * @author Timothy Scott */ @@ -45,7 +50,10 @@ namespace microservice_versions { -constexpr size_t NUM_SERVICES = 3; +// NUM_SERVICES should be one greater than the actual number of supported services, because index 0 contains +// dummy metadata for any unrecognized service. +constexpr size_t NUM_SERVICES = 4; +// service ID to represent an unknown service constexpr uint16_t MAVLINK_SERVICE_UNKNOWN = 0; /// This struct represents the information about microservice versions that is known to PX4 at compile time. @@ -84,6 +92,11 @@ struct service_status { uint16_t selected_version; }; +/// Contains the actual data about what versions of what services are supported. +/// When adding support for a new microservice, or a new version of an existing microservice, you should +/// change the definition of this array in `mavlink_service_versions.cpp`. +/// TODO microservice version: Determine if there is a better way to declare this. If I define it in this header file, +/// it can potentially take up more flash space, as it will be redefined for every file in which it is included. extern const service_metadata services_metadata[NUM_SERVICES]; // /// Returns the metadata of a given microservice. From 64411b77a195022c17aa438d00de0417b53d396b Mon Sep 17 00:00:00 2001 From: Timothy Scott Date: Fri, 13 Dec 2019 14:01:00 +0100 Subject: [PATCH 7/8] Changed to use MAV_CMD_REQUEST_SERVICE_VERSION --- src/modules/mavlink/mavlink_main.cpp | 65 ------- src/modules/mavlink/mavlink_main.h | 42 +---- src/modules/mavlink/mavlink_messages.cpp | 109 +----------- src/modules/mavlink/mavlink_mission.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 13 +- .../mavlink/mavlink_service_versions.cpp | 166 ++++++++++++++++++ .../mavlink/mavlink_service_versions.h | 109 +++++++++++- 7 files changed, 296 insertions(+), 210 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 03c8dd06af56..66d5677c8bcc 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -183,15 +183,6 @@ Mavlink::Mavlink() : if (_first_start_time == 0) { _first_start_time = hrt_absolute_time(); } - - // Initialize microservice versions - for (size_t i = 0; i < microservice_versions::NUM_SERVICES; i++) { - _microservice_versions[i] = { - .metadata = µservice_versions::services_metadata[i], - .status = microservice_versions::UNKNOWN, - .selected_version = 0 - }; - } } Mavlink::~Mavlink() @@ -1247,62 +1238,6 @@ Mavlink::send_protocol_version() set_proto_version(curr_proto_ver); } -microservice_versions::service_status & -Mavlink::get_service_status(uint16_t service_id) -{ - // TODO microservice versions: Maybe we don't have to search, and can just always place service ID 2 at index 2 - for (size_t i = 1; i < microservice_versions::NUM_SERVICES; i++) { - if (_microservice_versions[i].metadata->service_id == service_id) { - return _microservice_versions[i]; - } - } - - return _microservice_versions[0]; -} - -microservice_versions::service_status & -Mavlink::determine_service_version(uint16_t service_id, uint16_t min_version, uint16_t max_version) -{ - microservice_versions::service_status &status = get_service_status(service_id); - - if (status.metadata->service_id == microservice_versions::MAVLINK_SERVICE_UNKNOWN) { - return status; - } - - if (status.metadata->max_version < min_version || status.metadata->min_version > max_version) { - status.status = microservice_versions::UNSUPPORTED; - status.selected_version = 0; - - } else { - // In this branch, we know that there is overlap, so just pick the smaller of the two max versions. - status.selected_version = status.metadata->max_version < max_version ? status.metadata->max_version : max_version; - status.status = microservice_versions::SELECTED; - } - - return status; -} - -microservice_versions::service_status & -Mavlink::determine_service_version(uint16_t service_id) -{ - microservice_versions::service_status &status = get_service_status(service_id); - - if (status.metadata->service_id == microservice_versions::MAVLINK_SERVICE_UNKNOWN) { - return status; - } - - if (status.metadata->max_version == 0) { - status.status = microservice_versions::UNSUPPORTED; - - } else { - status.status = microservice_versions::SELECTED; - } - - status.selected_version = status.metadata->max_version; - - return status; -} - MavlinkOrbSubscription * Mavlink::add_orb_subscription(const orb_id_t topic, int instance, bool disable_sharing) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 8e82e074446b..61ad039c48c3 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -409,39 +409,15 @@ class Mavlink : public ModuleParams */ void send_protocol_version(); - /** - * This function returns the status of the given service, without selecting a version to use. - * @param service_id ID of the service in question - * @return Reference to a service_status which exists for the lifetime of this Mavlink object - */ - microservice_versions::service_status &get_service_status(uint16_t service_id); - - /** - * Takes the min and max version supported by the communication partner, and determines the maximum version - * supported by both this version of PX4 and the communication partner. This selected version is then - * stored in this instance of Mavlink, and can be retrieved with Mavlink::get_service_status. - * - * @param service_id ID of the service being negotiated - * @param min_version Minimum version of the service supported by the other system. - * @param max_version Maximum version of the service supported by the other system. - * @return Reference to a service_status, which exists for the lifetime of this Mavlink object, and has been - * modified with the results of this service version handshake. - */ - microservice_versions::service_status &determine_service_version(uint16_t service_id, uint16_t min_version, - uint16_t max_version); + void set_service_version_stream(microservice_versions::MavlinkServiceVersions *stream) + { + _service_version_stream = stream; + } - /** - * See Mavlink::determine_service_version(uint16_t, uint16_t, uint16_t). - * - * This overloaded function performs the same task as the other determine_service_version, but it does not know the - * min and max version supported by the other system, so it assumes that it supports every version (like a - * GCS would), and just selects the maximum version supported by this version of PX4. - * - * @param service_id ID of the service - * @return Reference to a service_status, which exists for the lifetime of this Mavlink object, and has been - * modified with the results of this service version handshake. - */ - microservice_versions::service_status &determine_service_version(uint16_t service_id); + microservice_versions::MavlinkServiceVersions *get_service_version_stream() + { + return _service_version_stream; + } List &get_streams() { return _streams; } @@ -694,7 +670,7 @@ class Mavlink : public ModuleParams pthread_mutex_t _message_buffer_mutex {}; pthread_mutex_t _send_mutex {}; - microservice_versions::service_status _microservice_versions[microservice_versions::NUM_SERVICES]; + microservice_versions::MavlinkServiceVersions *_service_version_stream{nullptr}; DEFINE_PARAMETERS( (ParamInt) _param_mav_sys_id, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 35e97625b095..cdde9399e73c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -5128,113 +5128,6 @@ class MavlinkStreamObstacleDistance : public MavlinkStream } }; -class MavlinkStreamServiceVersion : public MavlinkStream -{ -public: - const char *get_name() const override - { - return MavlinkStreamServiceVersion::get_name_static(); - } - - static const char *get_name_static() - { - return "MAVLINK_SERVICE_VERSION"; - } - - static uint16_t get_id_static() - { - return MAVLINK_MSG_ID_MAVLINK_SERVICE_VERSION; - } - - uint16_t get_id() override - { - return get_id_static(); - } - - static MavlinkStream *new_instance(Mavlink *mavlink) - { - return new MavlinkStreamServiceVersion(mavlink); - } - - unsigned get_size() override - { - return MAVLINK_MSG_ID_MAVLINK_SERVICE_VERSION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - } - -private: - - uint16_t _current_service = 0; - uint16_t _min_version = 0; - uint16_t _max_version = 0; - bool _sending_all_services = false; - int _default_interval = -1; - - /* do not allow top copying this class */ - MavlinkStreamServiceVersion(MavlinkStreamServiceVersion &) = delete; - MavlinkStreamServiceVersion &operator = (const MavlinkStreamServiceVersion &) = delete; - -protected: - explicit MavlinkStreamServiceVersion(Mavlink *mavlink) : MavlinkStream(mavlink) - {} - - bool send(const hrt_abstime t) override - { - if (_current_service > 0 && _current_service < microservice_versions::NUM_SERVICES) { - mavlink_mavlink_service_version_t msg; - - auto &status = _sending_all_services ? _mavlink->determine_service_version(_current_service) - : _mavlink->determine_service_version(_current_service, _min_version, _max_version); - - msg.service_id = _current_service; - msg.target_system = _mavlink->get_system_id(); - msg.target_component = _mavlink->get_component_id(); - // TODO microservice versioning: How does the handshake work in "stream all services" mode? - // The current implementation is to just select my maximum version and hope for the best. - msg.selected_version = status.selected_version; - msg.service_min_version = status.metadata->min_version; - msg.service_max_version = status.metadata->max_version; - msg.service_flags = status.metadata->min_version == 0 ? 0 : 1; - - mavlink_msg_mavlink_service_version_send_struct(_mavlink->get_channel(), &msg); - - if (_sending_all_services) { - _current_service++; - - } else { - _current_service = 0; - } - - return true; - - } else { - _default_interval = _interval; - set_interval(INT_MAX); - return false; - } - - } - - void request_message(float param1 = 0.0, float param2 = 0.0, float param3 = 0.0, float param4 = 0.0, - float param5 = 0.0, float param6 = 0.0, float param7 = 0.0) override - { - uint16_t service_id = (uint16_t)(param2 + 0.5f); - _min_version = (uint16_t)(param3 + 0.5f); - _max_version = (uint16_t)(param4 + 0.5f); - - if (service_id == 0) { - _sending_all_services = true; - _current_service = 1; - - } else { - _sending_all_services = false; - _current_service = service_id; - } - - set_interval(_default_interval); - reset_last_sent(); - } -}; - static const StreamListItem streams_list[] = { StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static, &MavlinkStreamHeartbeat::get_id_static), StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static, &MavlinkStreamStatustext::get_id_static), @@ -5293,7 +5186,7 @@ static const StreamListItem streams_list[] = { StreamListItem(&MavlinkStreamPing::new_instance, &MavlinkStreamPing::get_name_static, &MavlinkStreamPing::get_id_static), StreamListItem(&MavlinkStreamOrbitStatus::new_instance, &MavlinkStreamOrbitStatus::get_name_static, &MavlinkStreamOrbitStatus::get_id_static), StreamListItem(&MavlinkStreamObstacleDistance::new_instance, &MavlinkStreamObstacleDistance::get_name_static, &MavlinkStreamObstacleDistance::get_id_static), - StreamListItem(&MavlinkStreamServiceVersion::new_instance, &MavlinkStreamServiceVersion::get_name_static, &MavlinkStreamServiceVersion::get_id_static) + StreamListItem(µservice_versions::MavlinkServiceVersions::new_instance, µservice_versions::MavlinkServiceVersions::get_name_static, µservice_versions::MavlinkServiceVersions::get_id_static) }; const char *get_stream_name(const uint16_t msg_id) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 09615bf5a5aa..97baaefaab16 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -422,7 +422,7 @@ MavlinkMissionManager::current_item_count() bool MavlinkMissionManager::int_mode() { - auto status = _mavlink->get_service_status(MAVLINK_SERVICE_ID_MISSION); + auto status = _mavlink->get_service_version_stream()->get_service_status(MAVLINK_SERVICE_ID_MISSION); return status.selected_version > 1; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index afd18d4e3ff0..481c6ed6743e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -524,7 +524,8 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const for (const auto &stream : _mavlink->get_streams()) { if (stream->get_id() == message_id) { stream->request_message(vehicle_command.param1, vehicle_command.param2, vehicle_command.param3, - vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, vehicle_command.param7); + vehicle_command.param4, vehicle_command.param5, vehicle_command.param6, + vehicle_command.param7); stream_found = true; break; } @@ -535,6 +536,16 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED; } + } else if (cmd_mavlink.command == MAV_CMD_REQUEST_SERVICE_VERSION) { + if (_mavlink->get_service_version_stream()) { + _mavlink->get_service_version_stream()->request_serice_version((uint16_t) roundf(vehicle_command.param1), + (uint16_t) roundf(vehicle_command.param2), + (uint16_t) roundf(vehicle_command.param3)); + + } else { + PX4_ERR("Received MAV_CMD_REQUEST_SERVICE_VERSION, but the stream for MAVLINK_SERVICE_VERSION messages was not instantiated."); + } + } else { send_ack = false; diff --git a/src/modules/mavlink/mavlink_service_versions.cpp b/src/modules/mavlink/mavlink_service_versions.cpp index 0eb5b797b34b..609f2a700c09 100644 --- a/src/modules/mavlink/mavlink_service_versions.cpp +++ b/src/modules/mavlink/mavlink_service_versions.cpp @@ -38,6 +38,7 @@ * @author Timothy Scott */ +#include "mavlink_main.h" #include "mavlink_service_versions.h" // TODO microservice versioning: These will be replaced with the autogenerated MAVLink constants @@ -71,4 +72,169 @@ const service_metadata services_metadata[NUM_SERVICES] { } }; +service_status & +MavlinkServiceVersions::get_service_status(uint16_t service_id) +{ + // TODO microservice versions: Maybe we don't have to search, and can just always place service ID 2 at index 2 + for (size_t i = 1; i < microservice_versions::NUM_SERVICES; i++) { + if (_microservice_versions[i].metadata->service_id == service_id) { + return _microservice_versions[i]; + } + } + + return _microservice_versions[0]; +} + +service_status & +MavlinkServiceVersions::determine_service_version(uint16_t service_id, uint16_t min_version, + uint16_t max_version) +{ + microservice_versions::service_status &status = get_service_status(service_id); + + if (status.metadata->service_id == microservice_versions::MAVLINK_SERVICE_UNKNOWN) { + return status; + } + + if (status.metadata->max_version < min_version || status.metadata->min_version > max_version) { + status.status = microservice_versions::UNSUPPORTED; + status.selected_version = 0; + + } else { + // In this branch, we know that there is overlap, so just pick the smaller of the two max versions. + status.selected_version = + status.metadata->max_version < max_version ? status.metadata->max_version : max_version; + status.status = microservice_versions::SELECTED; + } + + return status; +} + +service_status & +MavlinkServiceVersions::determine_service_version(uint16_t service_id) +{ + microservice_versions::service_status &status = get_service_status(service_id); + + if (status.metadata->service_id == microservice_versions::MAVLINK_SERVICE_UNKNOWN) { + return status; + } + + if (status.metadata->max_version == 0) { + status.status = microservice_versions::UNSUPPORTED; + + } else { + status.status = microservice_versions::SELECTED; + } + + status.selected_version = status.metadata->max_version; + + return status; +} + + +void MavlinkServiceVersions::request_serice_version(uint16_t service_id, uint16_t min_version, + uint16_t max_version) +{ + _min_version = min_version; + _max_version = max_version; + + if (service_id == 0) { + _sending_all_services = true; + _current_service = 1; + + } else { + _sending_all_services = false; + _current_service = service_id; + } + + set_interval(_default_interval); + reset_last_sent(); +} + +const char *MavlinkServiceVersions::get_name() const +{ + return MavlinkServiceVersions::get_name_static(); +} + +const char *MavlinkServiceVersions::get_name_static() +{ + return "MAVLINK_SERVICE_VERSION"; +} + +uint16_t MavlinkServiceVersions::get_id_static() +{ + return MAVLINK_MSG_ID_MAVLINK_SERVICE_VERSION; +} + +uint16_t MavlinkServiceVersions::get_id() +{ + return get_id_static(); +} + +MavlinkStream *MavlinkServiceVersions::new_instance(Mavlink *mavlink) +{ + return new MavlinkServiceVersions(mavlink); +} + +unsigned MavlinkServiceVersions::get_size() +{ + return MAVLINK_MSG_ID_MAVLINK_SERVICE_VERSION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + +MavlinkServiceVersions::MavlinkServiceVersions(Mavlink *mavlink) : MavlinkStream(mavlink) +{ + _mavlink->set_service_version_stream(this); + + // Initialize microservice versions + for (size_t i = 0; i < microservice_versions::NUM_SERVICES; i++) { + _microservice_versions[i] = { + .metadata = µservice_versions::services_metadata[i], + .status = microservice_versions::UNKNOWN, + .selected_version = 0 + }; + } +} + +MavlinkServiceVersions::~MavlinkServiceVersions() +{ + _mavlink->set_service_version_stream(nullptr); +} + + +bool MavlinkServiceVersions::send(const hrt_abstime t) +{ + if (_current_service > 0 && _current_service < microservice_versions::NUM_SERVICES) { + mavlink_mavlink_service_version_t msg; + + auto &status = _sending_all_services ? determine_service_version(_current_service) + : determine_service_version(_current_service, _min_version, + _max_version); + + msg.service_id = _current_service; + msg.target_system = _mavlink->get_system_id(); + msg.target_component = _mavlink->get_component_id(); + // TODO microservice versioning: How does the handshake work in "stream all services" mode? + // The current implementation is to just select my maximum version and hope for the best. + msg.selected_version = status.selected_version; + msg.service_min_version = status.metadata->min_version; + msg.service_max_version = status.metadata->max_version; + msg.service_flags = status.metadata->min_version == 0 ? 0 : 1; + + mavlink_msg_mavlink_service_version_send_struct(_mavlink->get_channel(), &msg); + + if (_sending_all_services) { + _current_service++; + + } else { + _current_service = 0; + } + + return true; + + } else { + _default_interval = _interval; + set_interval(INT_MAX); + return false; + } + +} } \ No newline at end of file diff --git a/src/modules/mavlink/mavlink_service_versions.h b/src/modules/mavlink/mavlink_service_versions.h index b64278145c43..94606e10ccd3 100644 --- a/src/modules/mavlink/mavlink_service_versions.h +++ b/src/modules/mavlink/mavlink_service_versions.h @@ -46,6 +46,10 @@ #pragma once #include +#include +#include +#include "mavlink_main.h" +#include "mavlink_stream.h" namespace microservice_versions { @@ -99,6 +103,107 @@ struct service_status { /// it can potentially take up more flash space, as it will be redefined for every file in which it is included. extern const service_metadata services_metadata[NUM_SERVICES]; -// /// Returns the metadata of a given microservice. -// const service_metadata *get_metadata(uint16_t service_id); +/** + * This class manages the microservice versioning handshake. + * + * It inherits from MavlinkStream, so that it can support the "stream all services" request, without overloading + * the MAVLink connection with too many messages all at once. Normally, it is idle, and streams no messages. However, + * when the `request_service_version(...)` function is called, it becomes active, and sends one + * `MAVLINK_SERVICE_VERSION` message per cycle. + * + * Once it has sent either the one service requested, or all services (if all were requested), it becomes idle again. + */ +class MavlinkServiceVersions : public MavlinkStream +{ +public: + const char *get_name() const override; + + static const char *get_name_static(); + + static uint16_t get_id_static(); + + uint16_t get_id() override; + + static MavlinkStream *new_instance(Mavlink *mavlink); + + unsigned get_size() override; + + /** + * This function returns the status of the given service, without selecting a version to use. + * @param service_id ID of the service in question + * @return Reference to a service_status which exists for the lifetime of this Mavlink object + */ + service_status &get_service_status(uint16_t service_id); + + /** + * Takes the min and max version supported by the communication partner, and determines the maximum version + * supported by both this version of PX4 and the communication partner. This selected version is then + * stored in this instance of Mavlink, and can be retrieved with Mavlink::get_service_status. + * + * @param service_id ID of the service being negotiated + * @param min_version Minimum version of the service supported by the other system. + * @param max_version Maximum version of the service supported by the other system. + * @return Reference to a service_status, which exists for the lifetime of this Mavlink object, and has been + * modified with the results of this service version handshake. + */ + service_status &determine_service_version(uint16_t service_id, uint16_t min_version, + uint16_t max_version); + + /** + * See MavlinkStreamServiceVersion::determine_service_version(uint16_t, uint16_t, uint16_t). + * + * This overloaded function performs the same task as the other determine_service_version, but it does not know the + * min and max version supported by the other system, so it assumes that it supports every version (like a + * GCS would), and just selects the maximum version supported by this version of PX4. + * + * @param service_id ID of the service + * @return Reference to a service_status, which exists for the lifetime of this Mavlink object, and has been + * modified with the results of this service version handshake. + */ + service_status &determine_service_version(uint16_t service_id); + + /** + * This function should be called when we receive a MAV_CMD_REQUEST_SERVICE_VERSION. It performs the whole + * microservice version handshake: It chooses an appropriate version, and responds with a MAVLINK_SERVICE_VERSION + * message. After this is done, you can use MavlinkServiceVersions::get_service_status to determine the result + * of the handshake. + * + * @param service_id ID of the service to negotiate + * @param min_version Minimum supported version, sent from the other system + * @param max_version Maximum supported version, sent from the other system + */ + void request_serice_version(uint16_t service_id, uint16_t min_version, uint16_t max_version); + +protected: + explicit MavlinkServiceVersions(Mavlink *mavlink); + + ~MavlinkServiceVersions(); + + bool send(const hrt_abstime t) override; + + // TODO microservice versioning This should probably be removed before merging. But I am keeping it here for now + // just in case... + // (In a previous iteration of microservice versioning, it just used the generic `MAV_CMD_REQUEST_MSG`, but now + // it doesn't) +// void request_message(float param1 = 0.0, float param2 = 0.0, float param3 = 0.0, float param4 = 0.0, +// float param5 = 0.0, float param6 = 0.0, float param7 = 0.0) override +// { +// request_serice_version((uint16_t) roundf(param2), (uint16_t) roundf(param3), (uint16_t) roundf(param4)); +// } + +private: + + uint16_t _current_service = 0; + uint16_t _min_version = 0; + uint16_t _max_version = 0; + bool _sending_all_services = false; + int _default_interval = -1; + + service_status _microservice_versions[NUM_SERVICES]; + + /* do not allow top copying this class */ + MavlinkServiceVersions(MavlinkServiceVersions &) = delete; + + MavlinkServiceVersions &operator=(const MavlinkServiceVersions &) = delete; +}; } \ No newline at end of file From a2feda57af5430a7190685d446a65fa609b1f09f Mon Sep 17 00:00:00 2001 From: Timothy Scott Date: Fri, 13 Dec 2019 16:21:31 +0100 Subject: [PATCH 8/8] Changed mission to support old behavior if no microservice version handshake is done --- src/modules/mavlink/mavlink_mission.cpp | 48 +++++++++++-------- src/modules/mavlink/mavlink_mission.h | 2 +- .../mavlink/mavlink_service_versions.cpp | 2 + 3 files changed, 30 insertions(+), 22 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 97baaefaab16..50d8767d382b 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -423,7 +423,13 @@ bool MavlinkMissionManager::int_mode() { auto status = _mavlink->get_service_version_stream()->get_service_status(MAVLINK_SERVICE_ID_MISSION); - return status.selected_version > 1; + + if (status.status != microservice_versions::handshake_status::SELECTED) { + return _int_mode_fallback; + + } else { + return status.selected_version > 1; + } } void @@ -630,11 +636,11 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) if (wpa.type == MAV_MISSION_UNSUPPORTED) { if (int_mode()) { - //int_mode() = false; + _int_mode_fallback = false; send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); } else { - //int_mode() = true; + _int_mode_fallback = true; send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); } @@ -742,9 +748,9 @@ void MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) { // The request comes in the old float mode, so we switch to it. -// if (int_mode()) { -// int_mode() = false; -// } + if (int_mode()) { + _int_mode_fallback = false; + } handle_mission_request_both(msg); } @@ -753,9 +759,9 @@ void MavlinkMissionManager::handle_mission_request_int(const mavlink_message_t *msg) { // The request comes in the new int mode, so we switch to it. -// if (!int_mode()) { -// int_mode() = true; -// } + if (!int_mode()) { + _int_mode_fallback = true; + } handle_mission_request_both(msg); } @@ -978,10 +984,10 @@ MavlinkMissionManager::switch_to_idle_state() void MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) { -// if (int_mode()) { -// // It seems that we should be using the float mode, let's switch out of int mode. -// int_mode() = false; -// } + if (int_mode()) { + // It seems that we should be using the float mode, let's switch out of int mode. + _int_mode_fallback = false; + } handle_mission_item_both(msg); } @@ -989,10 +995,10 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) void MavlinkMissionManager::handle_mission_item_int(const mavlink_message_t *msg) { -// if (!int_mode()) { -// // It seems that we should be using the int mode, let's switch to it. -// int_mode() = true; -// } + if (!int_mode()) { + // It seems that we should be using the int mode, let's switch to it. + _int_mode_fallback = true; + } handle_mission_item_both(msg); } @@ -1279,10 +1285,10 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT))) { // Switch to int mode if that is what we are receiving -// if ((mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT || -// mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)) { -// int_mode() = true; -// } + if ((mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT || + mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)) { + _int_mode_fallback = true; + } if (int_mode()) { /* The argument is actually a mavlink_mission_item_int_t in int_mode. diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index faa4d451c006..59dafdc7f4a4 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -100,7 +100,7 @@ class MavlinkMissionManager uint8_t _reached_sent_count{0}; ///< last time when the vehicle reached a waypoint - bool _int_mode{false}; ///< Use accurate int32 instead of float + bool _int_mode_fallback{false}; ///< Use accurate int32 instead of float unsigned _filesystem_errcount{0}; ///< File system error count diff --git a/src/modules/mavlink/mavlink_service_versions.cpp b/src/modules/mavlink/mavlink_service_versions.cpp index 609f2a700c09..4c08e5066e02 100644 --- a/src/modules/mavlink/mavlink_service_versions.cpp +++ b/src/modules/mavlink/mavlink_service_versions.cpp @@ -134,6 +134,8 @@ MavlinkServiceVersions::determine_service_version(uint16_t service_id) void MavlinkServiceVersions::request_serice_version(uint16_t service_id, uint16_t min_version, uint16_t max_version) { + // TODO microservice versioning: Remove this + PX4_INFO("Sending version for service %d", service_id); _min_version = min_version; _max_version = max_version;