mavlink: tix mission manager to use MavlinkStream API

5 files changed, 97 insertions, 77 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 44329e0d2..df078325a 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -131,8 +131,6 @@ Mavlink::Mavlink() :
 	_streams(nullptr),
 	_mission_manager(nullptr),
 	_parameters_manager(nullptr),
-	_mission_pub(-1),
-	_mission_result_sub(-1),
 	_mode(MAVLINK_MODE_NORMAL),
 	_channel(MAVLINK_COMM_0),
 	_logbuffer {},
@@ -1255,12 +1253,6 @@ Mavlink::task_main(int argc, char *argv[])
 	/* start the MAVLink receiver */
 	_receive_thread = MavlinkReceiver::receive_start(this);
 
-	_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
-
-	/* create mission manager */
-	_mission_manager = new MavlinkMissionManager(this);
-	_mission_manager->set_verbose(_verbose);
-
 	_task_running = true;
 
 	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
@@ -1287,6 +1279,14 @@ Mavlink::task_main(int argc, char *argv[])
 	_parameters_manager->set_interval(interval_from_rate(30.0f));
 	LL_APPEND(_streams, _parameters_manager);
 
+	/* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on
+	 * remote requests rate. Rate specified here controls how much bandwidth we will reserve for
+	 * mission messages. */
+	_mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this);
+	_mission_manager->set_interval(interval_from_rate(10.0f));
+	_mission_manager->set_verbose(_verbose);
+	LL_APPEND(_streams, _mission_manager);
+
 	switch (_mode) {
 	case MAVLINK_MODE_NORMAL:
 		configure_stream("SYS_STATUS", 1.0f);
@@ -1315,12 +1315,8 @@ Mavlink::task_main(int argc, char *argv[])
 		break;
 	}
 
-	float base_rate_mult = _datarate / 1000.0f;
-
-	MavlinkRateLimiter fast_rate_limiter(30000.0f / base_rate_mult);
-
 	/* set main loop delay depending on data rate to minimize CPU overhead */
-	_main_loop_delay = MAIN_LOOP_DELAY / base_rate_mult;
+	_main_loop_delay = MAIN_LOOP_DELAY * 1000 / _datarate;
 
 	/* now the instance is fully initialized and we can bump the instance count */
 	LL_APPEND(_mavlink_instances, this);
@@ -1371,11 +1367,6 @@ Mavlink::task_main(int argc, char *argv[])
 			stream->update(t);
 		}
 
-		if (fast_rate_limiter.check(t)) {
-			// TODO missions
-			//_mission_manager->eventloop();
-		}
-
 		/* pass messages from other UARTs or FTP worker */
 		if (_passing_on || _ftp_on) {
 
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 87bb9d9ad..e3bad828f 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -289,8 +289,6 @@ private:
 	MavlinkMissionManager	*_mission_manager;
 	MavlinkParametersManager *_parameters_manager;
 
-	orb_advert_t		_mission_pub;
-	int			_mission_result_sub;
 	MAVLINK_MODE 		_mode;
 
 	mavlink_channel_t	_channel;
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index d17ccc6f9..7a761588a 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -59,8 +59,7 @@
 static const int ERROR = -1;
 
 
-MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
-	_mavlink(mavlink),
+MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
 	_state(MAVLINK_WPM_STATE_IDLE),
 	_time_last_recv(0),
 	_time_last_sent(0),
@@ -79,9 +78,8 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
 	_offboard_mission_sub(-1),
 	_mission_result_sub(-1),
 	_offboard_mission_pub(-1),
-	_slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()),
+	_slow_rate_limiter(_interval / 10.0f),
 	_verbose(false),
-	_channel(mavlink->get_channel()),
 	_comp_id(MAV_COMP_ID_MISSIONPLANNER)
 {
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
@@ -96,6 +94,20 @@ MavlinkMissionManager::~MavlinkMissionManager()
 	close(_mission_result_sub);
 }
 
+unsigned
+MavlinkMissionManager::get_size()
+{
+	if (_state == MAVLINK_WPM_STATE_SENDLIST) {
+		return MAVLINK_MSG_ID_MISSION_ITEM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+	} else if (_state == MAVLINK_WPM_STATE_GETLIST) {
+		return MAVLINK_MSG_ID_MISSION_REQUEST + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+	} else {
+		return 0;
+	}
+}
+
 void
 MavlinkMissionManager::init_offboard_mission()
 {
@@ -275,9 +287,10 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
 
 
 void
-MavlinkMissionManager::eventloop()
+MavlinkMissionManager::send(const hrt_abstime now)
 {
-	hrt_abstime now = hrt_absolute_time();
+	/* update interval for slow rate limiter */
+	_slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult());
 
 	bool updated = false;
 	orb_check(_mission_result_sub, &updated);
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
index 5bf80768c..8ff27f87d 100644
--- a/src/modules/mavlink/mavlink_mission.h
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -42,9 +42,11 @@
 
 #pragma once
 
+#include <uORB/uORB.h>
+
 #include "mavlink_bridge_header.h"
 #include "mavlink_rate_limiter.h"
-#include <uORB/uORB.h>
+#include "mavlink_stream.h"
 
 enum MAVLINK_WPM_STATES {
 	MAVLINK_WPM_STATE_IDLE = 0,
@@ -65,20 +67,75 @@ enum MAVLINK_WPM_CODES {
 #define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000    ///< Protocol communication action timeout in useconds
 #define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000        ///< Protocol communication retry timeout in useconds
 
-class Mavlink;
-
-class MavlinkMissionManager {
+class MavlinkMissionManager : public MavlinkStream {
 public:
-	MavlinkMissionManager(Mavlink *parent);
-
 	~MavlinkMissionManager();
 
+	const char *get_name() const
+	{
+		return MavlinkMissionManager::get_name_static();
+	}
+
+	static const char *get_name_static()
+	{
+		return "MISSION_ITEM";
+	}
+
+	uint8_t get_id()
+	{
+		return MAVLINK_MSG_ID_MISSION_ITEM;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink)
+	{
+		return new MavlinkMissionManager(mavlink);
+	}
+
+	unsigned get_size();
+
+	void handle_message(const mavlink_message_t *msg);
+
+	void set_verbose(bool v) { _verbose = v; }
+
+private:
+	enum MAVLINK_WPM_STATES _state;					///< Current state
+
+	uint64_t		_time_last_recv;
+	uint64_t		_time_last_sent;
+
+	uint32_t		_action_timeout;
+	uint32_t		_retry_timeout;
+	unsigned		_max_count;				///< Maximum number of mission items
+
+	int			_dataman_id;				///< Dataman storage ID for active mission
+	unsigned		_count;					///< Count of items in active mission
+	int			_current_seq;				///< Current item sequence in active mission
+
+	int			_transfer_dataman_id;			///< Dataman storage ID for current transmission
+	unsigned		_transfer_count;			///< Items count in current transmission
+	unsigned		_transfer_seq;				///< Item sequence in current transmission
+	unsigned		_transfer_current_seq;			///< Current item ID for current transmission (-1 means not initialized)
+	unsigned		_transfer_partner_sysid;		///< Partner system ID for current transmission
+	unsigned		_transfer_partner_compid;		///< Partner component ID for current transmission
+
+	int			_offboard_mission_sub;
+	int			_mission_result_sub;
+	orb_advert_t		_offboard_mission_pub;
+
+	MavlinkRateLimiter	_slow_rate_limiter;
+
+	bool _verbose;
+
+	uint8_t			_comp_id;
+
+	/* do not allow top copying this class */
+	MavlinkMissionManager(MavlinkMissionManager &);
+	MavlinkMissionManager& operator = (const MavlinkMissionManager &);
+
 	void init_offboard_mission();
 
 	int update_active_mission(int dataman_id, unsigned count, int seq);
 
-	void send_message(mavlink_message_t *msg);
-
 	/**
 	 *  @brief Sends an waypoint ack message
 	 */
@@ -110,10 +167,6 @@ public:
 	 */
 	void send_mission_item_reached(uint16_t seq);
 
-	void eventloop();
-
-	void handle_message(const mavlink_message_t *msg);
-
 	void handle_mission_ack(const mavlink_message_t *msg);
 
 	void handle_mission_set_current(const mavlink_message_t *msg);
@@ -138,43 +191,8 @@ public:
 	 */
 	int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
 
-	void set_verbose(bool v) { _verbose = v; }
-
-private:
-	Mavlink*		_mavlink;
-
-	enum MAVLINK_WPM_STATES _state;					///< Current state
-
-	uint64_t		_time_last_recv;
-	uint64_t		_time_last_sent;
-
-	uint32_t		_action_timeout;
-	uint32_t		_retry_timeout;
-	unsigned		_max_count;				///< Maximum number of mission items
-
-	int			_dataman_id;				///< Dataman storage ID for active mission
-	unsigned		_count;					///< Count of items in active mission
-	int			_current_seq;				///< Current item sequence in active mission
-
-	int			_transfer_dataman_id;			///< Dataman storage ID for current transmission
-	unsigned		_transfer_count;			///< Items count in current transmission
-	unsigned		_transfer_seq;				///< Item sequence in current transmission
-	unsigned		_transfer_current_seq;			///< Current item ID for current transmission (-1 means not initialized)
-	unsigned		_transfer_partner_sysid;		///< Partner system ID for current transmission
-	unsigned		_transfer_partner_compid;		///< Partner component ID for current transmission
-
-	int			_offboard_mission_sub;
-	int			_mission_result_sub;
-	orb_advert_t		_offboard_mission_pub;
-
-	MavlinkRateLimiter	_slow_rate_limiter;
-
-	bool _verbose;
-
-	mavlink_channel_t	_channel;
-	uint8_t			_comp_id;
+protected:
+	explicit MavlinkMissionManager(Mavlink *mavlink);
 
-	/* do not allow copying this class */
-	MavlinkMissionManager(const MavlinkMissionManager&);
-	MavlinkMissionManager operator=(const MavlinkMissionManager&);
+	void send(const hrt_abstime t);
 };
diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp
index 9cdda8b17..d3b3e1cde 100644
--- a/src/modules/mavlink/mavlink_rate_limiter.cpp
+++ b/src/modules/mavlink/mavlink_rate_limiter.cpp
@@ -64,7 +64,7 @@ MavlinkRateLimiter::check(hrt_abstime t)
 	uint64_t dt = t - _last_sent;
 
 	if (dt > 0 && dt >= _interval) {
-		_last_sent = (t / _interval) * _interval;
+		_last_sent = t;
 		return true;
 	}