Microservice Versioning Proof of Concept

.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

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

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);

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,

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)

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,15 +1577,15 @@ 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 {
 				mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
 			}
 
 		} else {
-			if (_int_mode) {
+			if (int_mode()) {
 				mavlink_mission_item->frame = MAV_FRAME_GLOBAL_INT;
 
 			} else {

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 &);

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;