From ca1f7a4a194c23303c23ca79b5905ff8bfb94c22 Mon Sep 17 00:00:00 2001
From: acfloria <florian.achermann@mavt.ethz.ch>
Date: Thu, 14 Jun 2018 12:43:06 +0200
Subject: [PATCH] Reenable the MissionManager for the Iridium mavlink instance

The MissionManager handles the MAVLINK_MSG_ID_MISSION_SET_CURRENT command which should also be supported for SatCom.
---
 src/modules/mavlink/mavlink_mission.cpp  | 10 ++++++++++
 src/modules/mavlink/mavlink_receiver.cpp | 21 +++++----------------
 src/modules/mavlink/mavlink_receiver.h   |  2 +-
 3 files changed, 16 insertions(+), 17 deletions(-)

diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 91b48f663f..00d12b9516 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -480,6 +480,11 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
 void
 MavlinkMissionManager::send(const hrt_abstime now)
 {
+	// do not send anything over high latency communication
+	if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
+		return;
+	}
+
 	bool updated = false;
 	orb_check(_mission_result_sub, &updated);
 
@@ -1651,6 +1656,11 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 
 void MavlinkMissionManager::check_active_mission()
 {
+	// do not send anything over high latency communication
+	if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
+		return;
+	}
+
 	if (!(_my_dataman_id == _dataman_id)) {
 		PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating");
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 70cbd4ac6b..f51393122e 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -100,7 +100,7 @@ using matrix::wrap_2pi;
 
 MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_mavlink(parent),
-	_mission_manager(nullptr),
+	_mission_manager(parent),
 	_parameters_manager(parent),
 	_mavlink_ftp(parent),
 	_mavlink_log_handler(parent),
@@ -163,20 +163,12 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_p_bat_crit_thr(param_find("BAT_CRIT_THR")),
 	_p_bat_low_thr(param_find("BAT_LOW_THR"))
 {
-	if (_mavlink->get_mode() != Mavlink::MAVLINK_MODE_IRIDIUM) {
-		_mission_manager = new MavlinkMissionManager(parent);
-	}
 }
 
 MavlinkReceiver::~MavlinkReceiver()
 {
 	orb_unsubscribe(_control_mode_sub);
 	orb_unsubscribe(_actuator_armed_sub);
-
-	if (_mission_manager != nullptr) {
-		delete _mission_manager;
-		_mission_manager = nullptr;
-	}
 }
 
 void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result)
@@ -2582,9 +2574,8 @@ MavlinkReceiver::receive_thread(void *arg)
 						handle_message(&msg);
 
 						/* handle packet with mission manager */
-						if (_mission_manager != nullptr) {
-							_mission_manager->handle_message(&msg);
-						}
+						_mission_manager.handle_message(&msg);
+
 
 						/* handle packet with parameter component */
 						_parameters_manager.handle_message(&msg);
@@ -2615,10 +2606,8 @@ MavlinkReceiver::receive_thread(void *arg)
 		hrt_abstime t = hrt_absolute_time();
 
 		if (t - last_send_update > timeout * 1000) {
-			if (_mission_manager != nullptr) {
-				_mission_manager->check_active_mission();
-				_mission_manager->send(t);
-			}
+			_mission_manager.check_active_mission();
+			_mission_manager.send(t);
 
 			_parameters_manager.send(t);
 
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 9bd9a7cc2d..165a5a4f54 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -194,7 +194,7 @@ private:
 
 	Mavlink	*_mavlink;
 
-	MavlinkMissionManager		*_mission_manager;
+	MavlinkMissionManager		_mission_manager;
 	MavlinkParametersManager	_parameters_manager;
 	MavlinkFTP			_mavlink_ftp;
 	MavlinkLogHandler		_mavlink_log_handler;