mavlink: mission manager moved to separate class and reworked

1 files changed, 18 insertions, 74 deletions

diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 5d7273abd..085a97d73 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -56,51 +56,7 @@
 #include "mavlink_orb_subscription.h"
 #include "mavlink_stream.h"
 #include "mavlink_messages.h"
-
-// FIXME XXX - TO BE MOVED TO XML
-enum MAVLINK_WPM_STATES {
-	MAVLINK_WPM_STATE_IDLE = 0,
-	MAVLINK_WPM_STATE_SENDLIST,
-	MAVLINK_WPM_STATE_SENDLIST_SENDWPS,
-	MAVLINK_WPM_STATE_GETLIST,
-	MAVLINK_WPM_STATE_GETLIST_GETWPS,
-	MAVLINK_WPM_STATE_GETLIST_GOTALL,
-	MAVLINK_WPM_STATE_ENUM_END
-};
-
-enum MAVLINK_WPM_CODES {
-	MAVLINK_WPM_CODE_OK = 0,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
-	MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
-	MAVLINK_WPM_CODE_ENUM_END
-};
-
-
-#define MAVLINK_WPM_MAX_WP_COUNT 255
-#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000    ///< Protocol communication action timeout in useconds
-#define MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT 500000        ///< Protocol communication retry timeout in useconds
-#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000      ///< When to send a new setpoint
-#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
-
-
-struct mavlink_wpm_storage {
-	uint16_t size;							///< Count of waypoints in active mission
-	uint16_t max_size;						///< Maximal count of waypoints
-	int dataman_id;							///< Dataman storage ID for active mission
-	enum MAVLINK_WPM_STATES current_state;	///< Current waypoint manager state
-	int16_t current_wp_id;					///< Waypoint ID in current transmission
-	uint16_t current_count;					///< Waypoints count in current transmission
-	uint8_t current_partner_sysid;
-	uint8_t current_partner_compid;
-	int current_dataman_id;					///< Dataman storage ID for current transmission
-	uint64_t timestamp_lastaction;
-	uint64_t timestamp_last_send_setpoint;
-	uint64_t timestamp_last_send_request;
-	uint32_t action_timeout;
-	uint32_t retry_timeout;
-};
+#include "mavlink_mission.h"
 
 
 class Mavlink
@@ -143,7 +99,7 @@ public:
 
 	static bool	instance_exists(const char *device_name, Mavlink *self);
 
-	static void	forward_message(mavlink_message_t *msg, Mavlink *self);
+	static void	forward_message(const mavlink_message_t *msg, Mavlink *self);
 
 	static int get_uart_fd(unsigned index);
 
@@ -168,10 +124,6 @@ public:
 
 	bool		get_forwarding_on() { return _forwarding_on; }
 
-	/**
-	 * Handle waypoint related messages.
-	 */
-	void mavlink_wpm_message_handler(const mavlink_message_t *msg);
 
 	static int start_helper(int argc, char *argv[]);
 
@@ -192,6 +144,10 @@ public:
 	 */
 	int		set_hil_enabled(bool hil_enabled);
 
+	void	send_message(const mavlink_message_t *msg);
+
+	void	handle_message(const mavlink_message_t *msg);
+
 	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
 
 	int get_instance_id();
@@ -209,6 +165,10 @@ public:
 
 	int get_mavlink_fd() { return _mavlink_fd; }
 
+	int send_statustext(const char *string);
+	int send_statustext(enum MAV_SEVERITY severity, const char *string);
+
+	float get_rate_mult();
 
 	/* Functions for waiting to start transmission until message received. */
 	void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
@@ -238,12 +198,12 @@ private:
 	MavlinkOrbSubscription *_subscriptions;
 	MavlinkStream *_streams;
 
+	MavlinkMissionManager *_mission_manager;
+
 	orb_advert_t	_mission_pub;
-	struct mission_s _mission;
-	struct mission_result_s _mission_result;
+	int			_mission_result_sub;
 	MAVLINK_MODE _mode;
 
-	uint8_t _mavlink_wpm_comp_id;
 	mavlink_channel_t _channel;
 
 	struct mavlink_logbuffer _logbuffer;
@@ -251,10 +211,6 @@ private:
 
 	pthread_t _receive_thread;
 
-	/* Allocate storage space for waypoints */
-	mavlink_wpm_storage _wpm_s;
-	mavlink_wpm_storage *_wpm;
-
 	bool _verbose;
 	bool _forwarding_on;
 	bool _passing_on;
@@ -336,20 +292,9 @@ private:
 
 	void mavlink_update_system();
 
-	void mavlink_waypoint_eventloop(uint64_t now);
-	void mavlink_wpm_send_waypoint_reached(uint16_t seq);
-	void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq);
-	void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq);
-	void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count);
-	void mavlink_wpm_send_waypoint_current(uint16_t seq);
-	void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type);
-	void mavlink_wpm_init(mavlink_wpm_storage *state);
-	int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
-	int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
-	void publish_mission();
-
-	void mavlink_missionlib_send_message(mavlink_message_t *msg);
-	int mavlink_missionlib_send_gcs_string(const char *string);
+	uint8_t get_system_id();
+
+	uint8_t get_component_id();
 
 	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
 
@@ -364,13 +309,13 @@ private:
 
 	int message_buffer_is_empty();
 
-	bool message_buffer_write(void *ptr, int size);
+	bool message_buffer_write(const void *ptr, int size);
 
 	int message_buffer_get_ptr(void **ptr, bool *is_part);
 
 	void message_buffer_mark_read(int n);
 
-	void pass_message(mavlink_message_t *msg);
+	void pass_message(const mavlink_message_t *msg);
 
 	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
 
@@ -378,5 +323,4 @@ private:
 	 * Main mavlink task.
 	 */
 	int		task_main(int argc, char *argv[]);
-
 };