Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Microservice Versioning Proof of Concept #13565

Closed

Conversation

AmeliaEScott
Copy link
Contributor

@AmeliaEScott AmeliaEScott commented Nov 22, 2019

Related:

This PR uses this common.xml, and this C library generated from it.

This is currently a minimum functioning prototype, meant to demonstrate the possible functionality of microservice versioning. If this is to be merged, there will likely be some necessary changes in response to community feedback.

@hamishwillee
Copy link
Contributor

@ItsTimmy Two points.

  1. The purpose of prototype is to evaluate and validate the proposed solution. IMO the preferred solution is the "alternative sequence" I proposed.
    Is that what you implemented? Does that seem better to you? If so I will update the proposal to match that instead.

  2. There is an open question in this design. Specifically, it is possible to support a microservice but omit some features on particular vehicle types. The example Lorenz suggested was a notional FW rally point that included landing behaviours. For the purpose of discussion a submarine might not implement that quite reasonably, but it implements everything else.

We didn't really resolve that - but clearly we need to identify things that are optional in some way and be able to query for support.

@julianoes
Copy link
Contributor

IMO the preferred solution is the "alternative sequence" I proposed.

That's your proposal but I don't see why we need to have two ways to do it. Yes, the original proposition needs one more message to be transmitted but it's clear and I don't understand why we should limit flexibility for an optimization that we don't know if we need it.

@hamishwillee
Copy link
Contributor

That's your proposal but I don't see why we need to have two ways to do it. Yes, the original proposition needs one more message to be transmitted but it's clear and I don't understand why we should limit flexibility for an optimization that we don't know if we need it.

  1. How is it that you think we are limiting flexibility with the new proposal?
  2. There should only be one way to do things. Either the old way or the new way.
  3. The new way is to my mind actually just better because it puts less burden on the autopilot.

I'm fine with either approach - they both make logical sense to me. But if I had to implement this I'd much rather do the second version.

Anyway, the important thing is that Tim as implementer is convinced of the rightness of a particular approach. I don't mind so much what that is, as long as he tells me what was done, so I can make sure the docs match.

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
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would say that's easier.

}
// if ((mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT ||
// mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)) {
// int_mode() = true;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Here comes the catch: we somehow still need to be compatible to older ground stations which do not support micro services yet. So, we need some sort of "if no microservice version chosen, then try to guess if int or not".

****************************************************************************/

/**
* @file mavlink_service_versions.c
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
* @file mavlink_service_versions.c
* @file mavlink_service_versions.cpp

@@ -0,0 +1,74 @@
/****************************************************************************
*
* Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
* Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019 PX4 Development Team. All rights reserved.

{
.service_id = MAVLINK_SERVICE_ID_CAMERA,
.min_version = 3,
.max_version = 30
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

😲

/**
* This enum represents the current status of the microservice version handshake for one particular microservice.
*/
enum handshake_status {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
enum handshake_status {
enum class HandshakeStatus {

@julianoes
Copy link
Contributor

@hamishwillee ok we're lacking the context here. I suggest we discuss it on Slack or in the Google Sheet.

@AmeliaEScott
Copy link
Contributor Author

My current implementation is as follows:

  • QGC sends a MAV_CMD_REQUEST_MESSAGE, with the service ID and min and max version
  • PX4 sends a MAVLINK_SERVICE_VERSION message, which contains PX4's selected_version, but also PX4's min and max version (These are only used for debugging purposes by QGC, because PX4 has already selected a version at this point).
  • QGC continues communicating with the selected version of the microservice.

I think this does not put any significant burden on the autopilot. I can of course test this more thoroughly, but right now it only requires ~3 bytes of RAM per service per MAVLink connection, and in exchange we get the flexibility to support multiple versions on PX4. For example, right now, PX4 effectively already supports 2 versions of the mission protocol (as I have implemented it).

As services are updated to newer versions, we can make the decision on a case-by-case basis whether to drop support for older versions of a service on PX4, if flash space or computational load demands it.

I have also implemented the 'stream all services' functionality (QGC sends MAV_CMD_REQUEST_MESSAGE with service_id = 0), but not fully. Right now, PX4 just selects its own maximum supported version, but I don't think this is the best way to do it. If the supported version on PX4 is just one version newer than on QGC, they won't be able to communicate. And as I previously mentioned, the overhead of supporting multiple versions of a service on PX4 is very small.

@AmeliaEScott AmeliaEScott changed the title Microservice Versioning Microservice Versioning Proof of Concept Feb 13, 2020
@stale
Copy link

stale bot commented May 13, 2020

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.

@stale stale bot added the stale label May 13, 2020
@stale stale bot removed the stale label May 14, 2020
@julianoes
Copy link
Contributor

We can re-open this if we decide to make another effort for it.

@julianoes julianoes closed this Nov 4, 2020
@julianoes
Copy link
Contributor

Here is the diff so we don't lose it:

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/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 82232256a0b7..66d5677c8bcc 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1613,6 +1613,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);
@@ -1652,6 +1653,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);
@@ -1697,6 +1699,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);
@@ -1765,6 +1768,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);
@@ -1797,6 +1801,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_main.h b/src/modules/mavlink/mavlink_main.h
index 96c1833821b8..61ad039c48c3 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,6 +409,16 @@ class Mavlink : public ModuleParams
 	 */
 	void			send_protocol_version();
 
+	void set_service_version_stream(microservice_versions::MavlinkServiceVersions *stream)
+	{
+		_service_version_stream = stream;
+	}
+
+	microservice_versions::MavlinkServiceVersions *get_service_version_stream()
+	{
+		return _service_version_stream;
+	}
+
 	List<MavlinkStream *> &get_streams() { return _streams; }
 
 	float			get_rate_mult() const { return _rate_mult; }
@@ -659,6 +670,8 @@ class Mavlink : public ModuleParams
 	pthread_mutex_t		_message_buffer_mutex {};
 	pthread_mutex_t		_send_mutex {};
 
+	microservice_versions::MavlinkServiceVersions *_service_version_stream{nullptr};
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
 		(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id,
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 7b607724dce3..cdde9399e73c 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 <commander/px4_custom_mode.h>
 #include <drivers/drv_pwm_output.h>
@@ -54,6 +55,7 @@
 #include <px4_platform_common/time.h>
 #include <systemlib/mavlink_log.h>
 #include <math.h>
+#include <limits.h>
 
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
@@ -5183,7 +5185,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(&microservice_versions::MavlinkServiceVersions::new_instance, &microservice_versions::MavlinkServiceVersions::get_name_static, &microservice_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 952abf3bf595..50d8767d382b 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -53,6 +53,9 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 
+// 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<mavlink_mission_item_t *>(&wp));
 
@@ -416,13 +419,26 @@ MavlinkMissionManager::current_item_count()
 	return _count[_mission_type];
 }
 
+bool
+MavlinkMissionManager::int_mode()
+{
+	auto status = _mavlink->get_service_version_stream()->get_service_status(MAVLINK_SERVICE_ID_MISSION);
+
+	if (status.status != microservice_versions::handshake_status::SELECTED) {
+		return _int_mode_fallback;
+
+	} else {
+		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 +635,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_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);
 					}
 
@@ -732,8 +748,8 @@ 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);
@@ -743,8 +759,8 @@ 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);
@@ -968,9 +984,9 @@ MavlinkMissionManager::switch_to_idle_state()
 void
 MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
 {
-	if (_int_mode) {
+	if (int_mode()) {
 		// It seems that we should be using the float mode, let's switch out of int mode.
-		_int_mode = false;
+		_int_mode_fallback = false;
 	}
 
 	handle_mission_item_both(msg);
@@ -979,9 +995,9 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
 void
 MavlinkMissionManager::handle_mission_item_int(const mavlink_message_t *msg)
 {
-	if (!_int_mode) {
+	if (!int_mode()) {
 		// It seems that we should be using the int mode, let's switch to it.
-		_int_mode = true;
+		_int_mode_fallback = true;
 	}
 
 	handle_mission_item_both(msg);
@@ -992,7 +1008,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 +1281,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;
+			_int_mode_fallback = 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 +1559,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 +1577,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 +1585,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..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
 
@@ -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_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 71c8849cd5aa..481c6ed6743e 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -517,6 +517,35 @@ 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 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
new file mode 100644
index 000000000000..4c08e5066e02
--- /dev/null
+++ b/src/modules/mavlink/mavlink_service_versions.cpp
@@ -0,0 +1,242 @@
+/****************************************************************************
+ *
+ *   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 <timothy@auterion.com>
+ */
+
+#include "mavlink_main.h"
+#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
+#define MAVLINK_SERVICE_ID_CAMERA 3
+
+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
+	},
+	{
+		.service_id = MAVLINK_SERVICE_ID_CAMERA,
+		.min_version = 3,
+		.max_version = 30
+	}
+};
+
+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)
+{
+	// TODO microservice versioning: Remove this
+	PX4_INFO("Sending version for service %d", service_id);
+	_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 = &microservice_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
new file mode 100644
index 000000000000..94606e10ccd3
--- /dev/null
+++ b/src/modules/mavlink/mavlink_service_versions.h
@@ -0,0 +1,209 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ * 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 <timothy@auterion.com>
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <limits.h>
+#include <math.h>
+#include "mavlink_main.h"
+#include "mavlink_stream.h"
+
+namespace microservice_versions
+{
+
+// 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.
+/// 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;
+};
+
+/// 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];
+
+/**
+ * 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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants