Changed mission to support old behavior if no microservice version handshake is done

Author: Timothy Scott

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,21 +984,21 @@ 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);
 }
 
 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.

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
 

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;