WIP state on getting MAVLink as a class, much of the work done, but does not compile yet

7 files changed, 402 insertions, 245 deletions

diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index aba8c98da..b143e62f0 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -86,42 +86,6 @@ mavlink_system_t mavlink_system = {
 }; // System ID, 1-255, Component/Subsystem ID, 1-255
 
 /*
- * Internal function to send the bytes through the right serial port
- */
-void
-mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
-{
-	int uart = -1;
-
-	switch (channel) {
-		case MAVLINK_COMM_0:
-			uart = Mavlink::get_uart_fd(0);
-		break;
-		case MAVLINK_COMM_1:
-			uart = Mavlink::get_uart_fd(1);
-		break;
-		case MAVLINK_COMM_2:
-			uart = Mavlink::get_uart_fd(2);
-		break;
-		case MAVLINK_COMM_3:
-			uart = Mavlink::get_uart_fd(3);
-		break;
-		case MAVLINK_COMM_4:
-			uart = Mavlink::get_uart_fd(4);
-		break;
-		case MAVLINK_COMM_5:
-			uart = Mavlink::get_uart_fd(5);
-		break;
-		case MAVLINK_COMM_6:
-			uart = Mavlink::get_uart_fd(6);
-		break;
-	}
-
-	write(uart, ch, (size_t)(sizeof(uint8_t) * length));
-
-}
-
-/*
  * Internal function to give access to the channel status for each channel
  */
 extern mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 339030a86..1c7986cbb 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -47,6 +47,7 @@
 #include <errno.h>
 #include <math.h>
 #include <poll.h>
+#include <termios.h>
 #include <time.h>
 #include <sys/ioctl.h>
 #include <drivers/device/device.h>
@@ -70,10 +71,10 @@
 #include <mavlink/mavlink_log.h>
 #include <sys/types.h>
 #include <sys/stat.h>
+#include <commander/px4_custom_mode.h>
 
 
 #include "mavlink_bridge_header.h"
-#include "mavlink_parameters.h"
 #include <uORB/uORB.h>
 #include "math.h" /* isinf / isnan checks */
 #include <assert.h>
@@ -87,7 +88,9 @@
 #include <systemlib/err.h>
 #include <sys/stat.h>
 #include <dataman/dataman.h>
-#include <waypoints.h>
+#include "mavlink_main.h"
+#include "mavlink_orb_listener.h"
+#include "mavlink_receiver.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -102,7 +105,49 @@ static const int ERROR = -1;
  */
 extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
 
+/*
+ * Internal function to send the bytes through the right serial port
+ */
+void
+mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
+{
+	int uart = -1;
 
+	switch (channel) {
+		case MAVLINK_COMM_0:
+			uart = Mavlink::get_uart_fd(0);
+		break;
+		case MAVLINK_COMM_1:
+			uart = Mavlink::get_uart_fd(1);
+		break;
+		case MAVLINK_COMM_2:
+			uart = Mavlink::get_uart_fd(2);
+		break;
+		case MAVLINK_COMM_3:
+			uart = Mavlink::get_uart_fd(3);
+		break;
+		#ifdef MAVLINK_COMM_4
+		case MAVLINK_COMM_4:
+			uart = Mavlink::get_uart_fd(4);
+		break;
+		#endif
+		#ifdef MAVLINK_COMM_5
+		case MAVLINK_COMM_5:
+			uart = Mavlink::get_uart_fd(5);
+		break;
+		#endif
+		#ifdef MAVLINK_COMM_6
+		case MAVLINK_COMM_6:
+			uart = Mavlink::get_uart_fd(6);
+		break;
+		#endif
+	}
+
+	write(uart, ch, (size_t)(sizeof(uint8_t) * length));
+
+}
+
+static void usage(void);
 
 namespace mavlink
 {
@@ -122,6 +167,8 @@ Mavlink::Mavlink(const char *port, unsigned baud_rate) :
 	_mavlink_hil_enabled(false)
 	// _params_sub(-1)
 {
+	wpm = &wpm_s;
+	fops.ioctl = (int (*)(file*, int, long unsigned int))&mavlink_dev_ioctl;
 	// _parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
 
 }
@@ -191,6 +238,15 @@ Mavlink* Mavlink::get_instance(unsigned instance)
 	return inst;
 }
 
+int Mavlink::get_uart_fd(unsigned index)
+{
+	Mavlink* inst = get_instance(index);
+	if (inst)
+		return inst->_mavlink_fd;
+
+	return -1;
+}
+
 void
 Mavlink::parameters_update()
 {
@@ -202,48 +258,12 @@ Mavlink::parameters_update()
 
 }
 
-/* XXX not widely used */
-uint8_t chan = MAVLINK_COMM_0;
-
-/* XXX probably should be in a header... */
-extern pthread_t receive_start(int uart);
-
-/* Allocate storage space for waypoints */
-static mavlink_wpm_storage wpm_s;
-mavlink_wpm_storage *wpm = &wpm_s;
-
-bool mavlink_hil_enabled = false;
-
-/* protocol interface */
-static int uart;
-static int baudrate;
-bool gcs_link = true;
-
-/* interface mode */
-static enum {
-	MAVLINK_INTERFACE_MODE_OFFBOARD,
-	MAVLINK_INTERFACE_MODE_ONBOARD
-} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD;
-
-static struct mavlink_logbuffer lb;
-
-static void mavlink_update_system(void);
-static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
-static void usage(void);
-int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval);
-
 /****************************************************************************
  * MAVLink text message logger
  ****************************************************************************/
 
-static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
-
-static const struct file_operations mavlink_fops = {
-	.ioctl = mavlink_dev_ioctl
-};
-
-static int
-mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
+int
+Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 {
 	static unsigned int total_counter = 0;
 
@@ -252,10 +272,11 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 	case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL:
 	case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
 			const char *txt = (const char *)arg;
-			struct mavlink_logmessage msg;
-			strncpy(msg.text, txt, sizeof(msg.text));
-			mavlink_logbuffer_write(&lb, &msg);
-			total_counter++;
+			printf("logmsg: %s\n", txt);
+			//struct mavlink_logmessage msg;
+			//strncpy(msg.text, txt, sizeof(msg.text));
+			//mavlink_logbuffer_write(&lb, &msg);
+			//total_counter++;
 			return OK;
 		}
 
@@ -264,7 +285,7 @@ mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
 	}
 }
 
-void mavlink_update_system(void)
+void Mavlink::mavlink_update_system(void)
 {
 	static bool initialized = false;
 	static param_t param_system_id;
@@ -301,7 +322,7 @@ void mavlink_update_system(void)
 	}
 }
 
-int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
+int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
 {
 	/* process baud rate */
 	int speed;
@@ -398,7 +419,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 }
 
 int
-set_hil_on_off(bool hil_enabled)
+Mavlink::set_hil_on_off(bool hil_enabled)
 {
 	int ret = OK;
 
@@ -427,13 +448,13 @@ set_hil_on_off(bool hil_enabled)
 			hil_rate_interval = 5;
 		}
 
-		orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
+		orb_set_interval(subs.spa_sub, hil_rate_interval);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
 	}
 
 	if (!hil_enabled && mavlink_hil_enabled) {
 		mavlink_hil_enabled = false;
-		orb_set_interval(mavlink_subs.spa_sub, 200);
+		orb_set_interval(subs.spa_sub, 200);
 
 	} else {
 		ret = ERROR;
@@ -443,7 +464,7 @@ set_hil_on_off(bool hil_enabled)
 }
 
 void
-get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
+Mavlink::get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
 {
 	/* reset MAVLink mode bitfield */
 	*mavlink_base_mode = 0;
@@ -518,7 +539,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 }
 
 
-int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
+int Mavlink::set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
 {
 	int ret = OK;
 
@@ -552,7 +573,7 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
 		orb_set_interval(subs->actuators_sub, min_interval);
 		orb_set_interval(subs->actuators_effective_sub, min_interval);
 		orb_set_interval(subs->spa_sub, min_interval);
-		orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval);
+		orb_set_interval(subs->rates_setpoint_sub, min_interval);
 		break;
 
 	case MAVLINK_MSG_ID_MANUAL_CONTROL:
@@ -575,34 +596,19 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
 
 extern mavlink_system_t mavlink_system;
 
-extern int mavlink_missionlib_send_message(mavlink_message_t *msg);
-extern int mavlink_missionlib_send_gcs_string(const char *string);
-
-/**
- * If the queue index is not at 0, the queue sending
- * logic will send parameters from the current index
- * to len - 1, the end of the param list.
- */
-static unsigned int mavlink_param_queue_index = 0;
-
-/**
- * Callback for param interface.
- */
-void mavlink_pm_callback(void *arg, param_t param);
-
-void mavlink_pm_callback(void *arg, param_t param)
+void Mavlink::mavlink_pm_callback(void *arg, param_t param)
 {
-	mavlink_pm_send_param(param);
+	//mavlink_pm_send_param(param);
 	usleep(*(unsigned int *)arg);
 }
 
-void mavlink_pm_send_all_params(unsigned int delay)
+void Mavlink::mavlink_pm_send_all_params(unsigned int delay)
 {
 	unsigned int dbuf = delay;
 	param_foreach(&mavlink_pm_callback, &dbuf, false);
 }
 
-int mavlink_pm_queued_send()
+int Mavlink::mavlink_pm_queued_send()
 {
 	if (mavlink_param_queue_index < param_count()) {
 		mavlink_pm_send_param(param_for_index(mavlink_param_queue_index));
@@ -614,22 +620,22 @@ int mavlink_pm_queued_send()
 	}
 }
 
-void mavlink_pm_start_queued_send()
+void Mavlink::mavlink_pm_start_queued_send()
 {
 	mavlink_param_queue_index = 0;
 }
 
-int mavlink_pm_send_param_for_index(uint16_t index)
+int Mavlink::mavlink_pm_send_param_for_index(uint16_t index)
 {
 	return mavlink_pm_send_param(param_for_index(index));
 }
 
-int mavlink_pm_send_param_for_name(const char *name)
+int Mavlink::mavlink_pm_send_param_for_name(const char *name)
 {
 	return mavlink_pm_send_param(param_find(name));
 }
 
-int mavlink_pm_send_param(param_t param)
+int Mavlink::mavlink_pm_send_param(param_t param)
 {
 	if (param == PARAM_INVALID) return 1;
 
@@ -679,11 +685,11 @@ int mavlink_pm_send_param(param_t param)
 					  mavlink_type,
 					  param_count(),
 					  param_get_index(param));
-	ret = mavlink_missionlib_send_message(&tx_msg);
-	return ret;
+	mavlink_missionlib_send_message(&tx_msg);
+	return OK;
 }
 
-void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
+void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
 {
 	switch (msg->msgid) {
 	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
@@ -748,7 +754,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
 	}
 }
 
-void publish_mission()
+void Mavlink::publish_mission()
 {
 	/* Initialize mission publication if necessary */
 	if (mission_pub < 0) {
@@ -759,7 +765,7 @@ void publish_mission()
 	}
 }
 
-int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
+int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
 {
 	/* only support global waypoints for now */
 	switch (mavlink_mission_item->frame) {
@@ -797,7 +803,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli
 	mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F);
 	mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
 	mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
-	mission_item->nav_cmd = mavlink_mission_item->command;
+	mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
 
 	mission_item->time_inside = mavlink_mission_item->param1;
 	mission_item->autocontinue = mavlink_mission_item->autocontinue;
@@ -807,7 +813,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli
 	return OK;
 }
 
-int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
+int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
 {
 	if (mission_item->altitude_is_relative) {
 		mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
@@ -838,7 +844,7 @@ int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *missio
 	return OK;
 }
 
-void mavlink_wpm_init(mavlink_wpm_storage *state)
+void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
 {
 	state->size = 0;
 	state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
@@ -854,7 +860,7 @@ void mavlink_wpm_init(mavlink_wpm_storage *state)
 /*
  *  @brief Sends an waypoint ack message
  */
-void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
+void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
 {
 	mavlink_message_t msg;
 	mavlink_mission_ack_t wpa;
@@ -878,7 +884,7 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
  *
  *  @param seq The waypoint sequence number the MAV should fly to.
  */
-void mavlink_wpm_send_waypoint_current(uint16_t seq)
+void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
 {
 	if (seq < wpm->size) {
 		mavlink_message_t msg;
@@ -897,7 +903,7 @@ void mavlink_wpm_send_waypoint_current(uint16_t seq)
 	}
 }
 
-void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count)
+void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count)
 {
 	mavlink_message_t msg;
 	mavlink_mission_count_t wpc;
@@ -912,7 +918,7 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou
 	if (verbose) warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system);
 }
 
-void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
+void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
 {
 
 	struct mission_item_s mission_item;
@@ -946,7 +952,7 @@ void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
 	}
 }
 
-void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
+void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
 {
 	if (seq < wpm->max_size) {
 		mavlink_message_t msg;
@@ -972,7 +978,7 @@ void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t s
  *
  *  @param seq The waypoint sequence number the MAV has reached.
  */
-void mavlink_wpm_send_waypoint_reached(uint16_t seq)
+void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq)
 {
 	mavlink_message_t msg;
 	mavlink_mission_item_reached_t wp_reached;
@@ -986,7 +992,7 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
 }
 
 
-void mavlink_waypoint_eventloop(uint64_t now)
+void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
 {
 	/* check for timed-out operations */
 	if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
@@ -1002,7 +1008,7 @@ void mavlink_waypoint_eventloop(uint64_t now)
 }
 
 
-void mavlink_wpm_message_handler(const mavlink_message_t *msg)
+void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 {
 	uint64_t now = hrt_absolute_time();
 
@@ -1377,7 +1383,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg)
 }
 
 void
-mavlink_missionlib_send_message(mavlink_message_t *msg)
+Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
 {
 	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
 
@@ -1387,7 +1393,7 @@ mavlink_missionlib_send_message(mavlink_message_t *msg)
 
 
 int
-mavlink_missionlib_send_gcs_string(const char *string)
+Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
 {
 	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
 	mavlink_statustext_t statustext;
@@ -1419,11 +1425,11 @@ mavlink_missionlib_send_gcs_string(const char *string)
 void
 Mavlink::task_main_trampoline(int argc, char *argv[])
 {
-	mavlink::g_mavlink->task_main();
+	mavlink::g_mavlink->task_main(argc, argv);
 }
 
-void
-Mavlink::task_main()
+int
+Mavlink::task_main(int argc, char *argv[])
 {
 	/* inform about start */
 	warnx("Initializing..");
@@ -1529,16 +1535,18 @@ Mavlink::task_main()
 		err(1, "could not open %s", device_name);
 
 	/* create the device node that's used for sending text log messages, etc. */
-	register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL);
+	register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);
 
 	/* Initialize system properties */
 	mavlink_update_system();
 
 	/* start the MAVLink receiver */
-	receive_thread = receive_start(uart);
+	MavlinkReceiver rcv(this);
+	receive_thread = rcv.receive_start(uart);
 
 	/* start the ORB receiver */
-	uorb_receive_thread = uorb_receive_start();
+	MavlinkOrbListener listener(this);
+	uorb_receive_thread = listener.uorb_receive_start();
 
 	/* initialize waypoint manager */
 	mavlink_wpm_init(wpm);
@@ -1546,57 +1554,57 @@ Mavlink::task_main()
 	/* all subscriptions are now active, set up initial guess about rate limits */
 	if (baudrate >= 230400) {
 		/* 200 Hz / 5 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 20);
 		/* 50 Hz / 20 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 30);
 		/* 20 Hz / 50 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
 		/* 10 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
 		/* 10 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
 
 	} else if (baudrate >= 115200) {
 		/* 20 Hz / 50 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 50);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 50);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
 		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
 		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200);
 		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
 
 	} else if (baudrate >= 57600) {
 		/* 10 Hz / 100 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 300);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300);
 		/* 10 Hz / 100 ms ATTITUDE */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 200);
 		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
 		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
 		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
 		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500);
 
 	} else {
 		/* very low baud rate, limit to 1 Hz / 1000 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_RAW_IMU, 1000);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
 		/* 1 Hz / 1000 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
 		/* 0.5 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
 		/* 0.1 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
+		set_mavlink_interval_limit(&subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
 	}
 
 	int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 8d6f0bd6f..244af04a6 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -73,6 +73,70 @@
 #include <drivers/drv_rc_input.h>
 #include <uORB/topics/navigation_capabilities.h>
 
+
+#include <nuttx/fs/fs.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
+};
+
+
+/* WAYPOINT MANAGER - MISSION LIB */
+
+#define MAVLINK_WPM_MAX_WP_COUNT 15
+#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE                                  ///< Enable double buffer and in-flight updates
+#ifndef MAVLINK_WPM_TEXT_FEEDBACK
+#define MAVLINK_WPM_TEXT_FEEDBACK 0                                                  ///< Report back status information as text
+#endif
+#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000         ///< Protocol communication 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 {
+        mavlink_mission_item_t waypoints[MAVLINK_WPM_MAX_WP_COUNT];      ///< Currently active waypoints
+#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
+        mavlink_mission_item_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT];  ///< Receive buffer for next waypoints
+#endif
+        uint16_t size;
+        uint16_t max_size;
+        uint16_t rcv_size;
+        enum MAVLINK_WPM_STATES current_state;
+        int16_t current_wp_id;                                                        ///< Waypoint in current transmission
+        int16_t current_active_wp_id;                                        ///< Waypoint the system is currently heading towards
+        uint16_t current_count;
+        uint8_t current_partner_sysid;
+        uint8_t current_partner_compid;
+        uint64_t timestamp_lastaction;
+        uint64_t timestamp_last_send_setpoint;
+        uint64_t timestamp_firstinside_orbit;
+        uint64_t timestamp_lastoutside_orbit;
+        uint32_t timeout;
+        uint32_t delay_setpoint;
+        float accept_range_yaw;
+        float accept_range_distance;
+        bool yaw_reached;
+        bool pos_reached;
+        bool idle;
+        int current_dataman_id;
+};
+
 class Mavlink
 {
 public:
@@ -104,6 +168,8 @@ public:
 
 	static Mavlink* get_instance(unsigned instance);
 
+	static int get_uart_fd(unsigned index);
+
 	struct mavlink_subscriptions {
 		int sensor_sub;
 		int att_sub;
@@ -149,12 +215,19 @@ public:
 	/** Actuator armed state */
 	struct actuator_armed_s armed;
 
+protected:
+	/**
+	 * Pointer to the default cdev file operations table; useful for
+	 * registering clone devices etc.
+	 */
+	struct file_operations	fops;
+	int		_mavlink_fd;
+
 private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
 	int		_mavlink_task;			/**< task handle for sensor task */
 
-	int		_mavlink_fd;
 	int		_mavlink_incoming_fd;		/**< file descriptor on which to receive incoming strings */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
@@ -165,10 +238,45 @@ private:
 
 	int		_params_sub;
 
-	orb_advert_t mission_pub = -1;
-struct mission_s mission;
+	orb_advert_t	mission_pub;
+	struct mission_s mission;
+	uint8_t missionlib_msg_buf[300]; //XXX MAGIC NUMBER
+	bool thread_running;
+	bool thread_should_exit;
+
+	uint8_t mavlink_wpm_comp_id;
+	mavlink_channel_t chan;
+
+//  XXX probably should be in a header... 
+// extern pthread_t receive_start(int uart);
 
-uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
+	pthread_t receive_thread;
+	pthread_t uorb_receive_thread;
+
+	/* Allocate storage space for waypoints */
+	mavlink_wpm_storage wpm_s;
+	mavlink_wpm_storage *wpm;
+
+	bool verbose;
+	bool mavlink_hil_enabled;
+	int uart;
+	int baudrate;
+	bool gcs_link;
+	/**
+	 * If the queue index is not at 0, the queue sending
+	 * logic will send parameters from the current index
+	 * to len - 1, the end of the param list.
+	 */
+	unsigned int mavlink_param_queue_index;
+
+	/* interface mode */
+	enum {
+		MAVLINK_INTERFACE_MODE_OFFBOARD,
+		MAVLINK_INTERFACE_MODE_ONBOARD
+	} mavlink_link_mode;
+
+	struct mavlink_logbuffer lb;
+	bool mavlink_link_termination_allowed;
 	
 	/**
 	 * Update our local parameter cache.
@@ -240,7 +348,38 @@ uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
 	 * mavlink_pm_queued_send().
 	 * @see 		mavlink_pm_queued_send()
 	 */
-	void mavlink_pm_start_queued_send(void);
+	void mavlink_pm_start_queued_send();
+
+	void mavlink_update_system();
+
+	void mavlink_wpm_message_handler(const mavlink_message_t *msg);
+	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);
+
+	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
+
+	int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval);
+
+	void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+
+	/**
+	 * Callback for param interface.
+	 */
+	static void mavlink_pm_callback(void *arg, param_t param);
+
+	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -248,8 +387,8 @@ uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
 	void		task_main_trampoline(int argc, char *argv[]);
 
 	/**
-	 * Main sensor collection task.
+	 * Main mavlink task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	int		task_main(int argc, char *argv[]) __attribute__((noreturn));
 
 };
diff --git a/src/modules/mavlink/mavlink_orb_listener.cpp b/src/modules/mavlink/mavlink_orb_listener.cpp
index 3edad0f16..e2675dfa7 100644
--- a/src/modules/mavlink/mavlink_orb_listener.cpp
+++ b/src/modules/mavlink/mavlink_orb_listener.cpp
@@ -661,8 +661,8 @@ MavlinkOrbListener::l_control_mode(const struct listener *l)
 				   mavlink_state);
 }
 
-static void *
-uorb_receive_thread(void *arg)
+void *
+MavlinkOrbListener::uorb_receive_thread(void *arg)
 {
 	/* Set thread name */
 	prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid());
@@ -712,7 +712,7 @@ uorb_receive_thread(void *arg)
 }
 
 pthread_t
-uorb_receive_start(void)
+MavlinkOrbListener::uorb_receive_start(void)
 {
 	/* --- SENSORS RAW VALUE --- */
 	mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
diff --git a/src/modules/mavlink/mavlink_orb_listener.h b/src/modules/mavlink/mavlink_orb_listener.h
index 32a174fcd..29e081b36 100644
--- a/src/modules/mavlink/mavlink_orb_listener.h
+++ b/src/modules/mavlink/mavlink_orb_listener.h
@@ -55,18 +55,21 @@ public:
 	 */
 	~MavlinkOrbListener();
 
-	/**
-	* Start the mavlink task.
-	 *
-	 * @return		OK on success.
-	 */
-	int		start();
+	// *
+	// * Start the mavlink task.
+	//  *
+	//  * @return		OK on success.
+	 
+	// int		start();
 
 	/**
 	 * Display the mavlink status.
 	 */
 	void		status();
 
+	pthread_t uorb_receive_start(void);
+	void * uorb_receive_thread(void *arg);
+
 private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
@@ -122,8 +125,6 @@ private:
 	struct vehicle_attitude_s att;
 	struct airspeed_s airspeed;
 
-	struct mavlink_subscriptions mavlink_subs;
-
 	int status_sub;
 	int rc_sub;
 
@@ -138,6 +139,6 @@ private:
 	 */
 	uint64_t last_sensor_timestamp;
 
-	hrt_abstime last_sent_vfr = 0;
+	hrt_abstime last_sent_vfr;
 
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 6881a2280..ab4074558 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -76,61 +76,42 @@
 __BEGIN_DECLS
 
 #include "mavlink_bridge_header.h"
-#include "waypoints.h"
-#include "orb_topics.h"
-#include "mavlink_parameters.h"
+// #include "waypoints.h"
+#include "mavlink_receiver.h"
+#include "mavlink_main.h"
 #include "util.h"
 
 __END_DECLS
 
-/* XXX should be in a header somewhere */
-extern "C" pthread_t receive_start(int uart);
-
-static void handle_message(mavlink_message_t *msg);
-static void *receive_thread(void *arg);
-
-static mavlink_status_t status;
-static struct vehicle_vicon_position_s vicon_position;
-static struct vehicle_command_s vcmd;
-static struct offboard_control_setpoint_s offboard_control_sp;
-
-struct vehicle_global_position_s hil_global_pos;
-struct vehicle_local_position_s hil_local_pos;
-struct vehicle_attitude_s hil_attitude;
-struct vehicle_gps_position_s hil_gps;
-struct sensor_combined_s hil_sensors;
-struct battery_status_s	hil_battery_status;
-static orb_advert_t pub_hil_global_pos = -1;
-static orb_advert_t pub_hil_local_pos = -1;
-static orb_advert_t pub_hil_attitude = -1;
-static orb_advert_t pub_hil_gps = -1;
-static orb_advert_t pub_hil_sensors = -1;
-static orb_advert_t pub_hil_gyro = -1;
-static orb_advert_t pub_hil_accel = -1;
-static orb_advert_t pub_hil_mag = -1;
-static orb_advert_t pub_hil_baro = -1;
-static orb_advert_t pub_hil_airspeed = -1;
-static orb_advert_t pub_hil_battery = -1;
-
-/* packet counter */
-static int hil_counter = 0;
-static int hil_frames = 0;
-static uint64_t old_timestamp = 0;
-
-static orb_advert_t cmd_pub = -1;
-static orb_advert_t flow_pub = -1;
-
-static orb_advert_t offboard_control_sp_pub = -1;
-static orb_advert_t vicon_position_pub = -1;
-static orb_advert_t telemetry_status_pub = -1;
-
-// variables for HIL reference position
-static int32_t lat0 = 0;
-static int32_t lon0 = 0;
-static double alt0 = 0;
-
-static void
-handle_message(mavlink_message_t *msg)
+void MavlinkReceiver::MavlinkReceiver() :
+	pub_hil_global_pos(-1),
+	pub_hil_local_pos(-1),
+	pub_hil_attitude(-1),
+	pub_hil_gps(-1),
+	pub_hil_sensors(-1),
+	pub_hil_gyro(-1),
+	pub_hil_accel(-1),
+	pub_hil_mag(-1),
+	pub_hil_baro(-1),
+	pub_hil_airspeed(-1),
+	pub_hil_battery(-1),
+	hil_counter(0),
+	hil_frames(0),
+	old_timestamp(0),
+	cmd_pub(-1),
+	flow_pub(-1),
+	offboard_control_sp_pub(-1),
+	vicon_position_pub(-1),
+	telemetry_status_pub(-1),
+	lat0(0),
+	lon0(0),
+	alt0(0)
+{
+
+}
+
+void
+MavlinkReceiver::handle_message(mavlink_message_t *msg)
 {
 	if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
 
@@ -804,8 +785,8 @@ handle_message(mavlink_message_t *msg)
 /**
  * Receive data from UART.
  */
-static void *
-receive_thread(void *arg)
+void *
+MavlinkReceiver::receive_thread(void *arg)
 {
 	int uart_fd = *((int *)arg);
 
@@ -851,8 +832,13 @@ receive_thread(void *arg)
 	return NULL;
 }
 
+void MavlinkReceiver::print_status()
+{
+
+}
+
 pthread_t
-receive_start(int uart)
+MavlinkReceiver::receive_start(int uart)
 {
 	pthread_attr_t receiveloop_attr;
 	pthread_attr_init(&receiveloop_attr);
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 88ae4b110..556b6f8ad 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -40,6 +40,36 @@
 
 #pragma once
 
+#include <systemlib/perf_counter.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/mission_item_triplet.h>
+#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/mission_item_triplet.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/debug_key_value.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
+
 class Mavlink;
 
 class MavlinkReceiver
@@ -65,7 +95,9 @@ public:
 	/**
 	 * Display the mavlink status.
 	 */
-	void		status();
+	void		print_status();
+
+	pthread_t receive_start(int uart);
 
 private:
 
@@ -75,14 +107,41 @@ private:
 
 	Mavlink*	_mavlink;
 
-	/**
-	 * Shim for calling task_main from task_create.
-	 */
-	void		task_main_trampoline(int argc, char *argv[]);
 
-	/**
-	 * Main sensor collection task.
-	 */
-	void		task_main() __attribute__((noreturn));
+	void handle_message(mavlink_message_t *msg);
+	void *receive_thread(void *arg);
+
+	mavlink_status_t status;
+	struct vehicle_vicon_position_s vicon_position;
+	struct vehicle_command_s vcmd;
+	struct offboard_control_setpoint_s offboard_control_sp;
+	struct vehicle_global_position_s hil_global_pos;
+	struct vehicle_local_position_s hil_local_pos;
+	struct vehicle_attitude_s hil_attitude;
+	struct vehicle_gps_position_s hil_gps;
+	struct sensor_combined_s hil_sensors;
+	struct battery_status_s	hil_battery_status;
+	orb_advert_t pub_hil_global_pos;
+	orb_advert_t pub_hil_local_pos;
+	orb_advert_t pub_hil_attitude;
+	orb_advert_t pub_hil_gps;
+	orb_advert_t pub_hil_sensors;
+	orb_advert_t pub_hil_gyro;
+	orb_advert_t pub_hil_accel;
+	orb_advert_t pub_hil_mag;
+	orb_advert_t pub_hil_baro;
+	orb_advert_t pub_hil_airspeed;
+	orb_advert_t pub_hil_battery;
+	int hil_counter;
+	int hil_frames;
+	uint64_t old_timestamp;
+	orb_advert_t cmd_pub;
+	orb_advert_t flow_pub;
+	orb_advert_t offboard_control_sp_pub;
+	orb_advert_t vicon_position_pub;
+	orb_advert_t telemetry_status_pub;
+	int32_t lat0;
+	int32_t lon0;
+	double alt0;
 
 };