Merge pull request #763 from PX4/beta_mavlink2

Mavlink multi-stream support

ROMFS/px4fmu_common/init.d/rc.usb

@@ -5,38 +5,11 @@
 
 echo "Starting MAVLink on this USB console"
 
-# Stop tone alarm
-tone_alarm stop
-
-#
-# Check for UORB
-#
-if uorb start
-then
-	echo "uORB started"
-fi
-
-# Tell MAVLink that this link is "fast"
-if mavlink stop
-then
-	echo "stopped other MAVLink instance"
-fi
-mavlink start -b 230400 -d /dev/ttyACM0
-
-# Stop commander
-if commander stop
-then
-	echo "Commander stopped"
-fi
-sleep 1
-
-# Start the commander
-if commander start
-then
-	echo "Commander started"
-fi
-
-echo "MAVLink started, exiting shell.."
+mavlink start -r 10000 -d /dev/ttyACM0
+# Enable a number of interesting streams we want via USB
+mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
+mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
+mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
 
 # Exit shell to make it available to MAVLink
 exit 

ROMFS/px4fmu_common/init.d/rcS

@@ -116,6 +116,8 @@ then
 	set PWM_MAX none
 	set MKBLCTRL_MODE none
 	set FMU_MODE pwm
+	set MAVLINK_FLAGS default
+	set EXIT_ON_END no
 	set MAV_TYPE none
 	set LOAD_DEFAULT_APPS yes
 	
@@ -394,28 +396,34 @@ then
 	#
 	# MAVLink
 	#
-	set EXIT_ON_END no
 	
-	if [ $HIL == yes ]
+	if [ $MAVLINK_FLAGS == default ]
 	then
-		sleep 1
-		mavlink start -b 230400 -d /dev/ttyACM0 
-		usleep 5000
-	else
-		if [ $TTYS1_BUSY == yes ]
+		if [ $HIL == yes ]
 		then
-			# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
-			mavlink start -d /dev/ttyS0
+			sleep 1
+			set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0"
 			usleep 5000
-			
-			# Exit from nsh to free port for mavlink
-			set EXIT_ON_END yes
 		else
-			# Start MAVLink on default port: ttyS1
-			mavlink start
-			usleep 5000
+			# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
+			if [ $TTYS1_BUSY == yes ]
+			then
+				# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
+				set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
+				usleep 5000
+				
+				# Exit from nsh to free port for mavlink
+				set EXIT_ON_END yes
+			else
+				# Start MAVLink on default port: ttyS1
+				set MAVLINK_FLAGS "-r 1000"
+				usleep 5000
+			fi
 		fi
 	fi
+
+	mavlink start $MAVLINK_FLAGS
+	usleep 5000
 	
 	#
 	# Start the datamanager
@@ -560,6 +568,7 @@ then
 
 	if [ $EXIT_ON_END == yes ]
 	then
+		echo "[init] Exit from nsh"
 		exit
 	fi
 

makefiles/config_px4fmu-v2_default.mk

@@ -72,7 +72,6 @@ MODULES		+= systemcmds/dumpfile
 MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
-MODULES		+= modules/mavlink_onboard
 MODULES		+= modules/gpio_led
 
 #

nuttx-configs/px4fmu-v1/nsh/defconfig

@@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=32
+CONFIG_NFILE_DESCRIPTORS=36
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=32
+CONFIG_NFILE_DESCRIPTORS=36
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
@@ -796,11 +796,11 @@ CONFIG_SCHED_WORKQUEUE=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_WORKPRIORITY=192
 CONFIG_SCHED_WORKPERIOD=5000
-CONFIG_SCHED_WORKSTACKSIZE=4000
+CONFIG_SCHED_WORKSTACKSIZE=2000
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKPERIOD=50000
-CONFIG_SCHED_LPWORKSTACKSIZE=4000
+CONFIG_SCHED_LPWORKSTACKSIZE=2000
 # CONFIG_LIB_KBDCODEC is not set
 # CONFIG_LIB_SLCDCODEC is not set
 

src/drivers/device/cdev.cpp

@@ -124,7 +124,7 @@ CDev::register_class_devname(const char *class_devname)
 			if (ret == OK) break;
 		} else {
 			char name[32];
-			snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
+			snprintf(name, sizeof(name), "%s%d", class_devname, class_instance);
 			ret = register_driver(name, &fops, 0666, (void *)this);
 			if (ret == OK) break;
 		}

src/drivers/gps/gps.cpp

@@ -232,6 +232,11 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCRESET:
 		cmd_reset();
 		break;
+
+	default:
+		/* give it to parent if no one wants it */
+		ret = CDev::ioctl(filp, cmd, arg);
+		break;
 	}
 
 	unlock();

src/drivers/px4io/px4io.cpp

@@ -1332,12 +1332,15 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
 	battery_status.discharged_mah = _battery_mamphour_total;
 	_battery_last_timestamp = battery_status.timestamp;
 
-	/* lazily publish the battery voltage */
-	if (_to_battery > 0) {
-		orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
+	/* the announced battery status would conflict with the simulated battery status in HIL */
+	if (!(_pub_blocked)) {
+		/* lazily publish the battery voltage */
+		if (_to_battery > 0) {
+			orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
 
-	} else {
-		_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
+		} else {
+			_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
+		}
 	}
 }
 
@@ -1959,8 +1962,7 @@ PX4IO::print_status()
 }
 
 int
-PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
-/* Make it obvious that file * isn't used here */
+PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 {
 	int ret = OK;
 
@@ -2372,8 +2374,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		break;
 
 	default:
-		/* not a recognized value */
-		ret = -ENOTTY;
+		/* see if the parent class can make any use of it */
+		ret = CDev::ioctl(filep, cmd, arg);
+		break;
 	}
 
 	return ret;

src/drivers/rgbled/rgbled.cpp

@@ -242,6 +242,8 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	default:
+		/* see if the parent class can make any use of it */
+		ret = CDev::ioctl(filp, cmd, arg);
 		break;
 	}
 

src/include/mavlink/mavlink_log.h

@@ -100,6 +100,7 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
  */
 #define mavlink_log_info(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
 
+
 struct mavlink_logmessage {
 	char text[MAVLINK_LOG_MAXLEN + 1];
 	unsigned char severity;
@@ -112,6 +113,7 @@ struct mavlink_logbuffer {
 	struct mavlink_logmessage *elems;
 };
 
+__BEGIN_DECLS
 void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size);
 
 void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb);
@@ -125,6 +127,7 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
 int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem);
 
 void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...);
+__END_DECLS
 
 #endif
 

src/lib/geo/geo.c

@@ -441,14 +441,14 @@ __EXPORT float _wrap_pi(float bearing)
 	}
 
 	int c = 0;
-	while (bearing > M_PI_F) {
+	while (bearing >= M_PI_F) {
 		bearing -= M_TWOPI_F;
 		if (c++ > 3)
 			return NAN;
 	}
 
 	c = 0;
-	while (bearing <= -M_PI_F) {
+	while (bearing < -M_PI_F) {
 		bearing += M_TWOPI_F;
 		if (c++ > 3)
 			return NAN;
@@ -465,14 +465,14 @@ __EXPORT float _wrap_2pi(float bearing)
 	}
 
 	int c = 0;
-	while (bearing > M_TWOPI_F) {
+	while (bearing >= M_TWOPI_F) {
 		bearing -= M_TWOPI_F;
 		if (c++ > 3)
 			return NAN;
 	}
 
 	c = 0;
-	while (bearing <= 0.0f) {
+	while (bearing < 0.0f) {
 		bearing += M_TWOPI_F;
 		if (c++ > 3)
 			return NAN;
@@ -489,14 +489,14 @@ __EXPORT float _wrap_180(float bearing)
 	}
 
 	int c = 0;
-	while (bearing > 180.0f) {
+	while (bearing >= 180.0f) {
 		bearing -= 360.0f;
 		if (c++ > 3)
 			return NAN;
 	}
 
 	c = 0;
-	while (bearing <= -180.0f) {
+	while (bearing < -180.0f) {
 		bearing += 360.0f;
 		if (c++ > 3)
 			return NAN;
@@ -513,14 +513,14 @@ __EXPORT float _wrap_360(float bearing)
 	}
 
 	int c = 0;
-	while (bearing > 360.0f) {
+	while (bearing >= 360.0f) {
 		bearing -= 360.0f;
 		if (c++ > 3)
 			return NAN;
 	}
 
 	c = 0;
-	while (bearing <= 0.0f) {
+	while (bearing < 0.0f) {
 		bearing += 360.0f;
 		if (c++ > 3)
 			return NAN;

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -277,7 +277,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	// XXX write this out to perf regs
 
 	/* keep track of sensor updates */
-	uint32_t sensor_last_count[3] = {0, 0, 0};
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
 	struct attitude_estimator_ekf_params ekf_params;
@@ -380,9 +379,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					uint8_t update_vect[3] = {0, 0, 0};
 
 					/* Fill in gyro measurements */
-					if (sensor_last_count[0] != raw.gyro_counter) {
+					if (sensor_last_timestamp[0] != raw.timestamp) {
 						update_vect[0] = 1;
-						sensor_last_count[0] = raw.gyro_counter;
 						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
@@ -392,11 +390,10 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
-					if (sensor_last_count[1] != raw.accelerometer_counter) {
+					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
 						update_vect[1] = 1;
-						sensor_last_count[1] = raw.accelerometer_counter;
 						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.timestamp;
+						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
 					}
 
 					hrt_abstime vel_t = 0;
@@ -445,11 +442,10 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
 
 					/* update magnetometer measurements */
-					if (sensor_last_count[2] != raw.magnetometer_counter) {
+					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
 						update_vect[2] = 1;
-						sensor_last_count[2] = raw.magnetometer_counter;
 						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-						sensor_last_timestamp[2] = raw.timestamp;
+						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
 					z_k[6] = raw.magnetometer_ga[0];

src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

@@ -445,7 +445,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 	// XXX write this out to perf regs
 
 	/* keep track of sensor updates */
-	uint32_t sensor_last_count[3] = {0, 0, 0};
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
 	struct attitude_estimator_so3_params so3_comp_params;
@@ -526,9 +525,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 					uint8_t update_vect[3] = {0, 0, 0};
 
 					/* Fill in gyro measurements */
-					if (sensor_last_count[0] != raw.gyro_counter) {
+					if (sensor_last_timestamp[0] != raw.timestamp) {
 						update_vect[0] = 1;
-						sensor_last_count[0] = raw.gyro_counter;
 						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
@@ -538,11 +536,10 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 					gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
-					if (sensor_last_count[1] != raw.accelerometer_counter) {
+					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
 						update_vect[1] = 1;
-						sensor_last_count[1] = raw.accelerometer_counter;
 						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.timestamp;
+						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
 					}
 
 					acc[0] = raw.accelerometer_m_s2[0];
@@ -550,11 +547,10 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 					acc[2] = raw.accelerometer_m_s2[2];
 
 					/* update magnetometer measurements */
-					if (sensor_last_count[2] != raw.magnetometer_counter) {
+					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
 						update_vect[2] = 1;
-						sensor_last_count[2] = raw.magnetometer_counter;
 						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-						sensor_last_timestamp[2] = raw.timestamp;
+						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
 					mag[0] = raw.magnetometer_ga[0];

src/modules/commander/px4_custom_mode.h

@@ -8,6 +8,8 @@
 #ifndef PX4_CUSTOM_MODE_H_
 #define PX4_CUSTOM_MODE_H_
 
+#include <stdint.h>
+
 enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
 	PX4_CUSTOM_MAIN_MODE_SEATBELT,

src/modules/commander/state_machine_helper.cpp

@@ -44,6 +44,7 @@
 #include <stdbool.h>
 #include <dirent.h>
 #include <fcntl.h>
+#include <string.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
@@ -309,10 +310,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 	bool valid_transition = false;
 	int ret = ERROR;
 
-	warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
-
 	if (current_status->hil_state == new_state) {
-		warnx("Hil state not changed");
 		valid_transition = true;
 
 	} else {
@@ -340,23 +338,60 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 
 				/* list directory */
 				DIR		*d;
-				struct dirent	*direntry;
 				d = opendir("/dev");
 				if (d) {
 
+					struct dirent	*direntry;
+					char devname[24];
+
 					while ((direntry = readdir(d)) != NULL) {
 
-						int sensfd = ::open(direntry->d_name, 0);
-						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
+						/* skip serial ports */
+						if (!strncmp("tty", direntry->d_name, 3)) {
+							continue;
+						}
+						/* skip mtd devices */
+						if (!strncmp("mtd", direntry->d_name, 3)) {
+							continue;
+						}
+						/* skip ram devices */
+						if (!strncmp("ram", direntry->d_name, 3)) {
+							continue;
+						}
+						/* skip MMC devices */
+						if (!strncmp("mmc", direntry->d_name, 3)) {
+							continue;
+						}
+						/* skip mavlink */
+						if (!strcmp("mavlink", direntry->d_name)) {
+							continue;
+						}
+						/* skip console */
+						if (!strcmp("console", direntry->d_name)) {
+							continue;
+						}
+						/* skip null */
+						if (!strcmp("null", direntry->d_name)) {
+							continue;
+						}
+
+						snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
+
+						int sensfd = ::open(devname, 0);
+
+						if (sensfd < 0) {
+							warn("failed opening device %s", devname);
+							continue;
+						}
+
+						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
 						close(sensfd);
 
-						printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
+						printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
 					}
 
 					closedir(d);
 
-					warnx("directory listing ok (FS mounted and readable)");
-
 				} else {
 					/* failed opening dir */
 					warnx("FAILED LISTING DEVICE ROOT DIRECTORY");

src/modules/mavlink/mavlink.c

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,46 +33,18 @@
 
 /**
  * @file mavlink.c
- * MAVLink 1.0 protocol implementation.
+ * Adapter functions expected by the protocol library
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #include <nuttx/config.h>
 #include <unistd.h>
-#include <pthread.h>
 #include <stdio.h>
-#include <math.h>
 #include <stdbool.h>
-#include <fcntl.h>
-#include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <poll.h>
-
 #include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/err.h>
-#include <mavlink/mavlink_log.h>
-#include <commander/px4_custom_mode.h>
-
-#include "waypoints.h"
-#include "orb_topics.h"
-#include "mavlink_hil.h"
-#include "util.h"
-#include "waypoints.h"
-#include "mavlink_parameters.h"
-
-#include <uORB/topics/mission_result.h>
 
 /* define MAVLink specific parameters */
 /**
@@ -92,22 +63,6 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
  */
 PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
 
-__EXPORT int mavlink_main(int argc, char *argv[]);
-
-static int mavlink_thread_main(int argc, char *argv[]);
-
-/* thread state */
-volatile bool thread_should_exit = false;
-static volatile bool thread_running = false;
-static int mavlink_task;
-
-/* pthreads */
-static pthread_t receive_thread;
-static pthread_t uorb_receive_thread;
-
-/* terminate MAVLink on user request - disabled by default */
-static bool mavlink_link_termination_allowed = false;
-
 mavlink_system_t mavlink_system = {
 	100,
 	50,
@@ -117,362 +72,6 @@ mavlink_system_t mavlink_system = {
 	0
 }; // System ID, 1-255, Component/Subsystem ID, 1-255
 
-/* 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);
-
-
-
-int
-set_hil_on_off(bool hil_enabled)
-{
-	int ret = OK;
-
-	/* Enable HIL */
-	if (hil_enabled && !mavlink_hil_enabled) {
-
-		mavlink_hil_enabled = true;
-
-		/* ramp up some HIL-related subscriptions */
-		unsigned hil_rate_interval;
-
-		if (baudrate < 19200) {
-			/* 10 Hz */
-			hil_rate_interval = 100;
-
-		} else if (baudrate < 38400) {
-			/* 10 Hz */
-			hil_rate_interval = 100;
-
-		} else if (baudrate < 115200) {
-			/* 20 Hz */
-			hil_rate_interval = 50;
-
-		} else {
-			/* 200 Hz */
-			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);
-	}
-
-	if (!hil_enabled && mavlink_hil_enabled) {
-		mavlink_hil_enabled = false;
-		orb_set_interval(mavlink_subs.spa_sub, 200);
-
-	} else {
-		ret = ERROR;
-	}
-
-	return ret;
-}
-
-void
-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;
-	*mavlink_custom_mode = 0;
-
-	/**
-	 * Set mode flags
-	 **/
-
-	/* HIL */
-	if (v_status.hil_state == HIL_STATE_ON) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
-	}
-
-	/* arming state */
-	if (armed.armed) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-	}
-
-	/* main state */
-	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
-	union px4_custom_mode custom_mode;
-	custom_mode.data = 0;
-	if (pos_sp_triplet.nav_state == NAV_STATE_NONE) {
-		/* use main state when navigator is not active */
-		if (v_status.main_state == MAIN_STATE_MANUAL) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
-		} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
-		} else if (v_status.main_state == MAIN_STATE_EASY) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
-		} else if (v_status.main_state == MAIN_STATE_AUTO) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-		}
-	} else {
-		/* use navigation state when navigator is active */
-		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-		if (pos_sp_triplet.nav_state == NAV_STATE_READY) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-		} else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
-		} else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
-		} else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
-		} else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
-		}
-	}
-	*mavlink_custom_mode = custom_mode.data;
-
-	/**
-	 * Set mavlink state
-	 **/
-
-	/* set calibration state */
-	if (v_status.arming_state == ARMING_STATE_INIT
-			|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
-			|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
-		*mavlink_state = MAV_STATE_UNINIT;
-	} else if (v_status.arming_state == ARMING_STATE_ARMED) {
-		*mavlink_state = MAV_STATE_ACTIVE;
-	} else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
-		*mavlink_state = MAV_STATE_CRITICAL;
-	} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
-		*mavlink_state = MAV_STATE_STANDBY;
-	} else if (v_status.arming_state == ARMING_STATE_REBOOT) {
-		*mavlink_state = MAV_STATE_POWEROFF;
-	} else {
-		warnx("Unknown mavlink state");
-		*mavlink_state = MAV_STATE_CRITICAL;
-	}
-}
-
-
-int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
-{
-	int ret = OK;
-
-	switch (mavlink_msg_id) {
-	case MAVLINK_MSG_ID_SCALED_IMU:
-		/* sensor sub triggers scaled IMU */
-		orb_set_interval(subs->sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_HIGHRES_IMU:
-		/* sensor sub triggers highres IMU */
-		orb_set_interval(subs->sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_RAW_IMU:
-		/* sensor sub triggers RAW IMU */
-		orb_set_interval(subs->sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_ATTITUDE:
-		/* attitude sub triggers attitude */
-		orb_set_interval(subs->att_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
-		/* actuator_outputs triggers this message */
-		orb_set_interval(subs->act_0_sub, min_interval);
-		orb_set_interval(subs->act_1_sub, min_interval);
-		orb_set_interval(subs->act_2_sub, min_interval);
-		orb_set_interval(subs->act_3_sub, min_interval);
-		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);
-		break;
-
-	case MAVLINK_MSG_ID_MANUAL_CONTROL:
-		/* manual_control_setpoint triggers this message */
-		orb_set_interval(subs->man_control_sp_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
-		orb_set_interval(subs->debug_key_value, min_interval);
-		break;
-
-	default:
-		/* not found */
-		ret = ERROR;
-		break;
-	}
-
-	return ret;
-}
-
-
-/****************************************************************************
- * 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)
-{
-	static unsigned int total_counter = 0;
-
-	switch (cmd) {
-	case (int)MAVLINK_IOC_SEND_TEXT_INFO:
-	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++;
-			return OK;
-		}
-
-	default:
-		return ENOTTY;
-	}
-}
-
-#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
-{
-	/* process baud rate */
-	int speed;
-
-	switch (baud) {
-	case 0:      speed = B0;      break;
-
-	case 50:     speed = B50;     break;
-
-	case 75:     speed = B75;     break;
-
-	case 110:    speed = B110;    break;
-
-	case 134:    speed = B134;    break;
-
-	case 150:    speed = B150;    break;
-
-	case 200:    speed = B200;    break;
-
-	case 300:    speed = B300;    break;
-
-	case 600:    speed = B600;    break;
-
-	case 1200:   speed = B1200;   break;
-
-	case 1800:   speed = B1800;   break;
-
-	case 2400:   speed = B2400;   break;
-
-	case 4800:   speed = B4800;   break;
-
-	case 9600:   speed = B9600;   break;
-
-	case 19200:  speed = B19200;  break;
-
-	case 38400:  speed = B38400;  break;
-
-	case 57600:  speed = B57600;  break;
-
-	case 115200: speed = B115200; break;
-
-	case 230400: speed = B230400; break;
-
-	case 460800: speed = B460800; break;
-
-	case 921600: speed = B921600; break;
-
-	default:
-		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
-		return -EINVAL;
-	}
-
-	/* open uart */
-	warnx("UART is %s, baudrate is %d\n", uart_name, baud);
-	uart = open(uart_name, O_RDWR | O_NOCTTY);
-
-	/* Try to set baud rate */
-	struct termios uart_config;
-	int termios_state;
-	*is_usb = false;
-
-	/* Back up the original uart configuration to restore it after exit */
-	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
-		close(uart);
-		return -1;
-	}
-
-	/* Fill the struct for the new configuration */
-	tcgetattr(uart, &uart_config);
-
-	/* Clear ONLCR flag (which appends a CR for every LF) */
-	uart_config.c_oflag &= ~ONLCR;
-
-	/* USB serial is indicated by /dev/ttyACM0*/
-	if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
-
-		/* Set baud rate */
-		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-			warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
-			close(uart);
-			return -1;
-		}
-
-	}
-
-	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
-		close(uart);
-		return -1;
-	}
-
-	return uart;
-}
-
-void
-mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
-{
-	write(uart, ch, (size_t)(sizeof(uint8_t) * length));
-}
-
 /*
  * Internal function to give access to the channel status for each channel
  */
@@ -490,362 +89,3 @@ extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
 	static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
 	return &m_mavlink_buffer[channel];
 }
-
-void mavlink_update_system(void)
-{
-	static bool initialized = false;
-	static param_t param_system_id;
-	static param_t param_component_id;
-	static param_t param_system_type;
-
-	if (!initialized) {
-		param_system_id = param_find("MAV_SYS_ID");
-		param_component_id = param_find("MAV_COMP_ID");
-		param_system_type = param_find("MAV_TYPE");
-		initialized = true;
-	}
-
-	/* update system and component id */
-	int32_t system_id;
-	param_get(param_system_id, &system_id);
-
-	if (system_id > 0 && system_id < 255) {
-		mavlink_system.sysid = system_id;
-	}
-
-	int32_t component_id;
-	param_get(param_component_id, &component_id);
-
-	if (component_id > 0 && component_id < 255) {
-		mavlink_system.compid = component_id;
-	}
-
-	int32_t system_type;
-	param_get(param_system_type, &system_type);
-
-	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
-		mavlink_system.type = system_type;
-	}
-}
-
-/**
- * MAVLink Protocol main function.
- */
-int mavlink_thread_main(int argc, char *argv[])
-{
-	/* initialize mavlink text message buffering */
-	mavlink_logbuffer_init(&lb, 10);
-
-	int ch;
-	char *device_name = "/dev/ttyS1";
-	baudrate = 57600;
-
-	/* work around some stupidity in task_create's argv handling */
-	argc -= 2;
-	argv += 2;
-
-	while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
-		switch (ch) {
-		case 'b':
-			baudrate = strtoul(optarg, NULL, 10);
-
-			if (baudrate < 9600 || baudrate > 921600)
-				errx(1, "invalid baud rate '%s'", optarg);
-
-			break;
-
-		case 'd':
-			device_name = optarg;
-			break;
-
-		case 'e':
-			mavlink_link_termination_allowed = true;
-			break;
-
-		case 'o':
-			mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
-			break;
-
-		default:
-			usage();
-			break;
-		}
-	}
-
-	struct termios uart_config_original;
-
-	bool usb_uart;
-
-	/* print welcome text */
-	warnx("MAVLink v1.0 serial interface starting...");
-
-	/* inform about mode */
-	warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
-
-	/* Flush stdout in case MAVLink is about to take it over */
-	fflush(stdout);
-
-	/* default values for arguments */
-	uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
-
-	if (uart < 0)
-		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);
-
-	/* Initialize system properties */
-	mavlink_update_system();
-
-	/* start the MAVLink receiver */
-	receive_thread = receive_start(uart);
-
-	/* start the ORB receiver */
-	uorb_receive_thread = uorb_receive_start();
-
-	/* initialize waypoint manager */
-	mavlink_wpm_init(wpm);
-
-	/* 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);
-		/* 50 Hz / 20 ms */
-		set_mavlink_interval_limit(&mavlink_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);
-		/* 10 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
-		/* 10 Hz */
-		set_mavlink_interval_limit(&mavlink_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);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_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);
-		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_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);
-		/* 10 Hz / 100 ms ATTITUDE */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_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);
-		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
-		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_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);
-		/* 1 Hz / 1000 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
-		/* 0.5 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
-		/* 0.1 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
-	}
-
-	int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
-	struct mission_result_s mission_result;
-	memset(&mission_result, 0, sizeof(mission_result));
-
-	thread_running = true;
-
-	/* arm counter to go off immediately */
-	unsigned lowspeed_counter = 10;
-
-	while (!thread_should_exit) {
-
-		/* 1 Hz */
-		if (lowspeed_counter == 10) {
-			mavlink_update_system();
-
-			/* translate the current system state to mavlink state and mode */
-			uint8_t mavlink_state = 0;
-			uint8_t mavlink_base_mode = 0;
-			uint32_t mavlink_custom_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-
-			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
-
-			/* switch HIL mode if required */
-			if (v_status.hil_state == HIL_STATE_ON)
-				set_hil_on_off(true);
-			else if (v_status.hil_state == HIL_STATE_OFF)
-				set_hil_on_off(false);
-
-			/* send status (values already copied in the section above) */
-			mavlink_msg_sys_status_send(chan,
-						    v_status.onboard_control_sensors_present,
-						    v_status.onboard_control_sensors_enabled,
-						    v_status.onboard_control_sensors_health,
-						    v_status.load * 1000.0f,
-						    v_status.battery_voltage * 1000.0f,
-						    v_status.battery_current * 100.0f,
-						    v_status.battery_remaining * 100.0f,
-						    v_status.drop_rate_comm,
-						    v_status.errors_comm,
-						    v_status.errors_count1,
-						    v_status.errors_count2,
-						    v_status.errors_count3,
-						    v_status.errors_count4);
-			lowspeed_counter = 0;
-		}
-
-		lowspeed_counter++;
-
-		bool updated;
-		orb_check(mission_result_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
-
-			if (mission_result.mission_reached) {
-				mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index);
-			}
-		}
-
-		mavlink_waypoint_eventloop(hrt_absolute_time());
-
-		/* sleep quarter the time */
-		usleep(25000);
-
-		/* check if waypoint has been reached against the last positions */
-		mavlink_waypoint_eventloop(hrt_absolute_time());
-
-		/* sleep quarter the time */
-		usleep(25000);
-
-		/* send parameters at 20 Hz (if queued for sending) */
-		mavlink_pm_queued_send();
-		mavlink_waypoint_eventloop(hrt_absolute_time());
-
-		/* sleep quarter the time */
-		usleep(25000);
-
-		mavlink_waypoint_eventloop(hrt_absolute_time());
-
-		if (baudrate > 57600) {
-			mavlink_pm_queued_send();
-		}
-
-		/* sleep 10 ms */
-		usleep(10000);
-
-		/* send one string at 10 Hz */
-		if (!mavlink_logbuffer_is_empty(&lb)) {
-			struct mavlink_logmessage msg;
-			int lb_ret = mavlink_logbuffer_read(&lb, &msg);
-
-			if (lb_ret == OK) {
-				mavlink_missionlib_send_gcs_string(msg.text);
-			}
-		}
-
-		/* sleep 15 ms */
-		usleep(15000);
-	}
-
-	/* wait for threads to complete */
-	pthread_join(receive_thread, NULL);
-	pthread_join(uorb_receive_thread, NULL);
-
-	/* Reset the UART flags to original state */
-	tcsetattr(uart, TCSANOW, &uart_config_original);
-
-	/* destroy log buffer */
-	//mavlink_logbuffer_destroy(&lb);
-
-	thread_running = false;
-
-	return 0;
-}
-
-static void
-usage()
-{
-	fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n"
-		"       mavlink stop\n"
-		"       mavlink status\n");
-	exit(1);
-}
-
-int mavlink_main(int argc, char *argv[])
-{
-
-	if (argc < 2) {
-		warnx("missing command");
-		usage();
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		/* this is not an error */
-		if (thread_running)
-			errx(0, "mavlink already running");
-
-		thread_should_exit = false;
-		mavlink_task = task_spawn_cmd("mavlink",
-					  SCHED_DEFAULT,
-					  SCHED_PRIORITY_DEFAULT,
-					  2048,
-					  mavlink_thread_main,
-					  (const char **)argv);
-
-		while (!thread_running) {
-			usleep(200);
-		}
-
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-
-		/* this is not an error */
-		if (!thread_running)
-			errx(0, "mavlink already stopped");
-
-		thread_should_exit = true;
-
-		while (thread_running) {
-			usleep(200000);
-			warnx(".");
-		}
-
-		warnx("terminated.");
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			errx(0, "running");
-
-		} else {
-			errx(1, "not running");
-		}
-	}
-
-	warnx("unrecognized command");
-	usage();
-	/* not getting here */
-	return 0;
-}
-

src/modules/mavlink/mavlink_bridge_header.h

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,6 +42,8 @@
 #ifndef MAVLINK_BRIDGE_HEADER_H
 #define MAVLINK_BRIDGE_HEADER_H
 
+__BEGIN_DECLS
+
 #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
 
 /* use efficient approach, see mavlink_helpers.h */
@@ -73,11 +74,13 @@ extern mavlink_system_t mavlink_system;
  * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
  * @param ch Character to send
  */
-extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
+void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
 
 extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
 extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
 
 #include <v1.0/common/mavlink.h>
 
+__END_DECLS
+
 #endif /* MAVLINK_BRIDGE_HEADER_H */

src/modules/mavlink/mavlink_main.h

@@ -0,0 +1,325 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_main.h
+ * MAVLink 1.0 protocol interface definition.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <nuttx/fs/fs.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+#include <pthread.h>
+#include <mavlink/mavlink_log.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+
+#include "mavlink_bridge_header.h"
+#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 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;
+	uint16_t max_size;
+	enum MAVLINK_WPM_STATES current_state;
+	int16_t current_wp_id;	///< Waypoint in current transmission
+	uint16_t current_count;
+	uint8_t current_partner_sysid;
+	uint8_t current_partner_compid;
+	uint64_t timestamp_lastaction;
+	uint64_t timestamp_last_send_setpoint;
+	uint32_t timeout;
+	int current_dataman_id;
+};
+
+
+class Mavlink
+{
+
+public:
+	/**
+	 * Constructor
+	 */
+	Mavlink();
+
+	/**
+	 * Destructor, also kills the mavlinks task.
+	 */
+	~Mavlink();
+
+	/**
+	* Start the mavlink task.
+	 *
+	 * @return		OK on success.
+	 */
+	static int		start(int argc, char *argv[]);
+
+	/**
+	 * Display the mavlink status.
+	 */
+	void		status();
+
+	static int stream(int argc, char *argv[]);
+
+	static int	instance_count();
+
+	static Mavlink *new_instance();
+
+	static Mavlink *get_instance(unsigned instance);
+
+	static Mavlink *get_instance_for_device(const char *device_name);
+
+	static int	destroy_all_instances();
+
+	static bool	instance_exists(const char *device_name, Mavlink *self);
+
+	static int get_uart_fd(unsigned index);
+
+	int get_uart_fd();
+
+	const char *_device_name;
+
+	enum MAVLINK_MODE {
+		MAVLINK_MODE_NORMAL = 0,
+		MAVLINK_MODE_CUSTOM,
+		MAVLINK_MODE_CAMERA
+	};
+
+	void		set_mode(enum MAVLINK_MODE);
+	enum MAVLINK_MODE		get_mode() { return _mode; }
+
+	bool		get_hil_enabled() { return _hil_enabled; };
+
+	bool		get_flow_control_enabled() { return _flow_control_enabled; }
+
+	/**
+	 * Handle waypoint related messages.
+	 */
+	void mavlink_wpm_message_handler(const mavlink_message_t *msg);
+
+	static int start_helper(int argc, char *argv[]);
+
+	/**
+	 * Handle parameter related messages.
+	 */
+	void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
+
+	void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+
+	/**
+	 * Enable / disable Hardware in the Loop simulation mode.
+	 *
+	 * @param hil_enabled	The new HIL enable/disable state.
+	 * @return		OK if the HIL state changed, ERROR if the
+	 *			requested change could not be made or was
+	 *			redundant.
+	 */
+	int		set_hil_enabled(bool hil_enabled);
+
+	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
+
+	int get_instance_id();
+
+	/**
+	 * Enable / disable hardware flow control.
+	 *
+	 * @param enabled	True if hardware flow control should be enabled
+	 */
+	int enable_flow_control(bool enabled);
+
+	mavlink_channel_t get_channel();
+
+	bool		_task_should_exit;		/**< if true, mavlink task should exit */
+
+protected:
+	Mavlink	*next;
+
+private:
+	int _instance_id;
+
+	int		_mavlink_fd;
+	bool		_task_running;
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	/* states */
+	bool		_hil_enabled;		/**< Hardware In the Loop mode */
+	bool		_is_usb_uart;		/**< Port is USB */
+
+	unsigned	_main_loop_delay;		/**< mainloop delay, depends on data rate */
+
+	MavlinkOrbSubscription *_subscriptions;
+	MavlinkStream *_streams;
+
+	orb_advert_t	_mission_pub;
+	struct mission_s mission;
+	uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
+	MAVLINK_MODE _mode;
+
+	uint8_t _mavlink_wpm_comp_id;
+	mavlink_channel_t _channel;
+
+	struct mavlink_logbuffer _logbuffer;
+	unsigned int _total_counter;
+
+	pthread_t _receive_thread;
+
+	/* Allocate storage space for waypoints */
+	mavlink_wpm_storage _wpm_s;
+	mavlink_wpm_storage *_wpm;
+
+	bool _verbose;
+	int _uart_fd;
+	int _baudrate;
+	int _datarate;
+
+	/**
+	 * 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;
+
+	bool mavlink_link_termination_allowed;
+
+	char 	*_subscribe_to_stream;
+	float	_subscribe_to_stream_rate;
+
+	bool		_flow_control_enabled;
+
+	/**
+	 * Send one parameter.
+	 *
+	 * @param param		The parameter id to send.
+	 * @return		zero on success, nonzero on failure.
+	 */
+	int mavlink_pm_send_param(param_t param);
+
+	/**
+	 * Send one parameter identified by index.
+	 *
+	 * @param index		The index of the parameter to send.
+	 * @return		zero on success, nonzero else.
+	 */
+	int mavlink_pm_send_param_for_index(uint16_t index);
+
+	/**
+	 * Send one parameter identified by name.
+	 *
+	 * @param name		The index of the parameter to send.
+	 * @return		zero on success, nonzero else.
+	 */
+	int mavlink_pm_send_param_for_name(const char *name);
+
+	/**
+	 * Send a queue of parameters, one parameter per function call.
+	 *
+	 * @return		zero on success, nonzero on failure
+	 */
+	int mavlink_pm_queued_send(void);
+
+	/**
+	 * Start sending the parameter queue.
+	 *
+	 * This function will not directly send parameters, but instead
+	 * activate the sending of one parameter on each call of
+	 * mavlink_pm_queued_send().
+	 * @see 		mavlink_pm_queued_send()
+	 */
+	void mavlink_pm_start_queued_send();
+
+	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);
+
+	int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
+
+	int configure_stream(const char *stream_name, const float rate);
+	void configure_stream_threadsafe(const char *stream_name, const float rate);
+
+	static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
+
+	/**
+	 * Main mavlink task.
+	 */
+	int		task_main(int argc, char *argv[]);
+
+};

src/modules/mavlink/mavlink_messages.h

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,20 +32,17 @@
  ****************************************************************************/
 
 /**
- * @file mavlink_hil.h
- * Hardware-in-the-loop simulation support.
+ * @file mavlink_messages.h
+ * MAVLink 1.0 message formatters definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#pragma once
+#ifndef MAVLINK_MESSAGES_H_
+#define MAVLINK_MESSAGES_H_
 
-extern bool mavlink_hil_enabled;
+#include "mavlink_stream.h"
 
-/**
- * Enable / disable Hardware in the Loop simulation mode.
- *
- * @param hil_enabled	The new HIL enable/disable state.
- * @return		OK if the HIL state changed, ERROR if the
- *			requested change could not be made or was
- *			redundant.
- */
-extern int set_hil_on_off(bool hil_enabled);
+extern MavlinkStream *streams_list[];
+
+#endif /* MAVLINK_MESSAGES_H_ */

src/modules/mavlink/mavlink_orb_subscription.cpp

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,51 +32,82 @@
  ****************************************************************************/
 
 /**
- * @file mavlink_bridge_header
- * MAVLink bridge header for UART access.
+ * @file mavlink_orb_subscription.cpp
+ * uORB subscription implementation.
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-/* MAVLink adapter header */
-#ifndef MAVLINK_BRIDGE_HEADER_H
-#define MAVLINK_BRIDGE_HEADER_H
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <uORB/uORB.h>
+#include <stdio.h>
 
-#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
+#include "mavlink_orb_subscription.h"
 
-/* use efficient approach, see mavlink_helpers.h */
-#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
+MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
+	_fd(orb_subscribe(_topic)),
+	_published(false),
+	_topic(topic),
+	_last_check(0),
+	next(nullptr)
+{
+	_data = malloc(topic->o_size);
+	memset(_data, 0, topic->o_size);
+}
 
-#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
-#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
+MavlinkOrbSubscription::~MavlinkOrbSubscription()
+{
+	close(_fd);
+	free(_data);
+}
 
-#include <v1.0/mavlink_types.h>
-#include <unistd.h>
+const orb_id_t
+MavlinkOrbSubscription::get_topic()
+{
+	return _topic;
+}
 
+void *
+MavlinkOrbSubscription::get_data()
+{
+	return _data;
+}
 
-/* Struct that stores the communication settings of this system.
-   you can also define / alter these settings elsewhere, as long
-   as they're included BEFORE mavlink.h.
-   So you can set the
+bool
+MavlinkOrbSubscription::update(const hrt_abstime t)
+{
+	if (_last_check == t) {
+		/* already checked right now, return result of the check */
+		return _updated;
 
-   mavlink_system.sysid = 100; // System ID, 1-255
-   mavlink_system.compid = 50; // Component/Subsystem ID, 1-255
+	} else {
+		_last_check = t;
+		orb_check(_fd, &_updated);
 
-   Lines also in your main.c, e.g. by reading these parameter from EEPROM.
- */
-extern mavlink_system_t mavlink_system;
+		if (_updated) {
+			orb_copy(_topic, _fd, _data);
+			return true;
+		}
+	}
 
-/**
- * @brief Send multiple chars (uint8_t) over a comm channel
- *
- * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
- * @param ch Character to send
- */
-extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length);
+	return false;
+}
+
+bool
+MavlinkOrbSubscription::is_published()
+{
+	if (_published) {
+		return true;
+	}
 
-mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
-mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
+	bool updated;
+	orb_check(_fd, &updated);
 
-#include <v1.0/common/mavlink.h>
+	if (updated) {
+		_published = true;
+	}
 
-#endif /* MAVLINK_BRIDGE_HEADER_H */
+	return _published;
+}

src/modules/mavlink/mavlink_orb_subscription.h

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_orb_subscription.h
+ * uORB subscription definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef MAVLINK_ORB_SUBSCRIPTION_H_
+#define MAVLINK_ORB_SUBSCRIPTION_H_
+
+#include <systemlib/uthash/utlist.h>
+#include <drivers/drv_hrt.h>
+
+
+class MavlinkOrbSubscription
+{
+public:
+	MavlinkOrbSubscription *next;	/*< pointer to next subscription in list */
+
+	MavlinkOrbSubscription(const orb_id_t topic);
+	~MavlinkOrbSubscription();
+
+	bool update(const hrt_abstime t);
+
+	/**
+	 * Check if the topic has been published.
+	 *
+	 * This call will return true if the topic was ever published.
+	 * @return true if the topic has been published at least once.
+	 */
+	bool is_published();
+	void *get_data();
+	const orb_id_t get_topic();
+
+private:
+	const orb_id_t _topic;		/*< topic metadata */
+	int _fd;					/*< subscription handle */
+	bool _published;			/*< topic was ever published */
+	void *_data;				/*< pointer to data buffer */
+	hrt_abstime _last_check;	/*< time of last check */
+	bool _updated;				/*< updated on last check */
+};
+
+
+#endif /* MAVLINK_ORB_SUBSCRIPTION_H_ */

src/modules/mavlink/mavlink_parameters.c

@@ -1,230 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file mavlink_parameters.c
- * MAVLink parameter protocol implementation (BSD-relicensed).
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include "mavlink_bridge_header.h"
-#include "mavlink_parameters.h"
-#include <uORB/uORB.h>
-#include "math.h" /* isinf / isnan checks */
-#include <assert.h>
-#include <stdio.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <stdbool.h>
-#include <string.h>
-#include <errno.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <sys/stat.h>
-
-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)
-{
-	mavlink_pm_send_param(param);
-	usleep(*(unsigned int *)arg);
-}
-
-void mavlink_pm_send_all_params(unsigned int delay)
-{
-	unsigned int dbuf = delay;
-	param_foreach(&mavlink_pm_callback, &dbuf, false);
-}
-
-int mavlink_pm_queued_send()
-{
-	if (mavlink_param_queue_index < param_count()) {
-		mavlink_pm_send_param(param_for_index(mavlink_param_queue_index));
-		mavlink_param_queue_index++;
-		return 0;
-
-	} else {
-		return 1;
-	}
-}
-
-void mavlink_pm_start_queued_send()
-{
-	mavlink_param_queue_index = 0;
-}
-
-int 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)
-{
-	return mavlink_pm_send_param(param_find(name));
-}
-
-int mavlink_pm_send_param(param_t param)
-{
-	if (param == PARAM_INVALID) return 1;
-
-	/* buffers for param transmission */
-	static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
-	float val_buf;
-	static mavlink_message_t tx_msg;
-
-	/* query parameter type */
-	param_type_t type = param_type(param);
-	/* copy parameter name */
-	strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-
-	/*
-	 * Map onboard parameter type to MAVLink type,
-	 * endianess matches (both little endian)
-	 */
-	uint8_t mavlink_type;
-
-	if (type == PARAM_TYPE_INT32) {
-		mavlink_type = MAVLINK_TYPE_INT32_T;
-
-	} else if (type == PARAM_TYPE_FLOAT) {
-		mavlink_type = MAVLINK_TYPE_FLOAT;
-
-	} else {
-		mavlink_type = MAVLINK_TYPE_FLOAT;
-	}
-
-	/*
-	 * get param value, since MAVLink encodes float and int params in the same
-	 * space during transmission, copy param onto float val_buf
-	 */
-
-	int ret;
-
-	if ((ret = param_get(param, &val_buf)) != OK) {
-		return ret;
-	}
-
-	mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
-					  mavlink_system.compid,
-					  MAVLINK_COMM_0,
-					  &tx_msg,
-					  name_buf,
-					  val_buf,
-					  mavlink_type,
-					  param_count(),
-					  param_get_index(param));
-	ret = mavlink_missionlib_send_message(&tx_msg);
-	return ret;
-}
-
-void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
-{
-	switch (msg->msgid) {
-	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
-			/* Start sending parameters */
-			mavlink_pm_start_queued_send();
-			mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
-		} break;
-
-	case MAVLINK_MSG_ID_PARAM_SET: {
-
-			/* Handle parameter setting */
-
-			if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) {
-				mavlink_param_set_t mavlink_param_set;
-				mavlink_msg_param_set_decode(msg, &mavlink_param_set);
-
-				if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter, set and send it */
-					param_t param = param_find(name);
-
-					if (param == PARAM_INVALID) {
-						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
-						sprintf(buf, "[mavlink pm] unknown: %s", name);
-						mavlink_missionlib_send_gcs_string(buf);
-
-					} else {
-						/* set and send parameter */
-						param_set(param, &(mavlink_param_set.param_value));
-						mavlink_pm_send_param(param);
-					}
-				}
-			}
-		} break;
-
-	case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
-			mavlink_param_request_read_t mavlink_param_request_read;
-			mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
-
-			if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
-				/* when no index is given, loop through string ids and compare them */
-				if (mavlink_param_request_read.param_index == -1) {
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter and send it */
-					mavlink_pm_send_param_for_name(name);
-
-				} else {
-					/* when index is >= 0, send this parameter again */
-					mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
-				}
-			}
-
-		} break;
-	}
-}

src/modules/mavlink/mavlink_parameters.h

@@ -1,104 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file mavlink_parameters.h
- * MAVLink parameter protocol definitions (BSD-relicensed).
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-/* This assumes you have the mavlink headers on your include path
- or in the same folder as this source file */
-
-
-#include <v1.0/mavlink_types.h>
-#include <stdbool.h>
-#include <systemlib/param/param.h>
-
-/**
- * Handle parameter related messages.
- */
-void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
-
-/**
- * Send all parameters at once.
- *
- * This function blocks until all parameters have been sent.
- * it delays each parameter by the passed amount of microseconds.
- *
- * @param delay		The delay in us between sending all parameters.
- */
-void mavlink_pm_send_all_params(unsigned int delay);
-
-/**
- * Send one parameter.
- *
- * @param param		The parameter id to send.
- * @return		zero on success, nonzero on failure.
- */
-int mavlink_pm_send_param(param_t param);
-
-/**
- * Send one parameter identified by index.
- *
- * @param index		The index of the parameter to send.
- * @return		zero on success, nonzero else.
- */
-int mavlink_pm_send_param_for_index(uint16_t index);
-
-/**
- * Send one parameter identified by name.
- *
- * @param name		The index of the parameter to send.
- * @return		zero on success, nonzero else.
- */
-int mavlink_pm_send_param_for_name(const char *name);
-
-/**
- * Send a queue of parameters, one parameter per function call.
- *
- * @return		zero on success, nonzero on failure
- */
-int mavlink_pm_queued_send(void);
-
-/**
- * Start sending the parameter queue.
- *
- * This function will not directly send parameters, but instead
- * activate the sending of one parameter on each call of
- * mavlink_pm_queued_send().
- * @see 		mavlink_pm_queued_send()
- */
-void mavlink_pm_start_queued_send(void);

src/modules/mavlink/mavlink_rate_limiter.cpp

@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_rate_limiter.cpp
+ * Message rate limiter implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "mavlink_rate_limiter.h"
+
+MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000)
+{
+}
+
+MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval)
+{
+}
+
+MavlinkRateLimiter::~MavlinkRateLimiter()
+{
+}
+
+void
+MavlinkRateLimiter::set_interval(unsigned int interval)
+{
+	_interval = interval;
+}
+
+bool
+MavlinkRateLimiter::check(hrt_abstime t)
+{
+	uint64_t dt = t - _last_sent;
+
+	if (dt > 0 && dt >= _interval) {
+		_last_sent = (t / _interval) * _interval;
+		return true;
+	}
+
+	return false;
+}

src/modules/mavlink/mavlink_rate_limiter.h

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,23 +32,31 @@
  ****************************************************************************/
 
 /**
- * @file util.h
- * Utility and helper functions and data.
+ * @file mavlink_rate_limiter.h
+ * Message rate limiter definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#pragma once
+#ifndef MAVLINK_RATE_LIMITER_H_
+#define MAVLINK_RATE_LIMITER_H_
 
-#include "orb_topics.h"
+#include <drivers/drv_hrt.h>
 
-/** MAVLink communications channel */
-extern uint8_t chan;
 
-/** Shutdown marker */
-extern volatile bool thread_should_exit;
+class MavlinkRateLimiter
+{
+private:
+	hrt_abstime _last_sent;
+	hrt_abstime _interval;
 
-/**
- * Translate the custom state into standard mavlink modes and state.
- */
-extern void
-get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
-	uint8_t *mavlink_state, uint8_t *mavlink_mode);
+public:
+	MavlinkRateLimiter();
+	MavlinkRateLimiter(unsigned int interval);
+	~MavlinkRateLimiter();
+	void set_interval(unsigned int interval);
+	bool check(hrt_abstime t);
+};
+
+
+#endif /* MAVLINK_RATE_LIMITER_H_ */

src/modules/mavlink/mavlink_receiver.h

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,12 +32,15 @@
  ****************************************************************************/
 
 /**
- * @file orb_topics.h
- * Common sets of topics subscribed to or published by the MAVLink driver,
- * and structures maintained by those subscriptions.
+ * @file mavlink_orb_listener.h
+ * MAVLink 1.0 uORB listener definition
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
+
 #pragma once
 
+#include <systemlib/perf_counter.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/rc_channels.h>
@@ -55,7 +57,6 @@
 #include <uORB/topics/vehicle_vicon_position.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>
@@ -66,59 +67,80 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
-#include <drivers/drv_rc_input.h>
-#include <uORB/topics/navigation_capabilities.h>
 
-struct mavlink_subscriptions {
-	int sensor_sub;
-	int att_sub;
-	int global_pos_sub;
-	int act_0_sub;
-	int act_1_sub;
-	int act_2_sub;
-	int act_3_sub;
-	int gps_sub;
-	int man_control_sp_sub;
-	int safety_sub;
-	int actuators_sub;
-	int armed_sub;
-	int actuators_effective_sub;
-	int local_pos_sub;
-	int spa_sub;
-	int spl_sub;
-	int triplet_sub;
-	int debug_key_value;
-	int input_rc_sub;
-	int optical_flow;
-	int rates_setpoint_sub;
-	int home_sub;
-	int airspeed_sub;
-	int navigation_capabilities_sub;
-	int position_setpoint_triplet_sub;
-};
+class Mavlink;
 
-extern struct mavlink_subscriptions mavlink_subs;
+class MavlinkReceiver
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MavlinkReceiver(Mavlink *parent);
 
-/** Global position */
-extern struct vehicle_global_position_s global_pos;
+	/**
+	 * Destructor, also kills the mavlinks task.
+	 */
+	~MavlinkReceiver();
 
-/** Local position */
-extern struct vehicle_local_position_s local_pos;
+	/**
+	* Start the mavlink task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
 
-/** navigation capabilities */
-extern struct navigation_capabilities_s nav_cap;
+	/**
+	 * Display the mavlink status.
+	 */
+	void		print_status();
 
-/** Vehicle status */
-extern struct vehicle_status_s v_status;
+	static pthread_t receive_start(Mavlink *parent);
 
-/** Position setpoint triplet */
-extern struct position_setpoint_triplet_s pos_sp_triplet;
+	static void *start_helper(void *context);
 
-/** RC channels */
-extern struct rc_channels_s rc;
+private:
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-/** Actuator armed state */
-extern struct actuator_armed_s armed;
+	Mavlink	*_mavlink;
 
-/** Worker thread starter */
-extern pthread_t uorb_receive_start(void);
+	void handle_message(mavlink_message_t *msg);
+	void handle_message_command_long(mavlink_message_t *msg);
+	void handle_message_optical_flow(mavlink_message_t *msg);
+	void handle_message_set_mode(mavlink_message_t *msg);
+	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
+	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
+	void handle_message_radio_status(mavlink_message_t *msg);
+	void handle_message_manual_control(mavlink_message_t *msg);
+	void handle_message_hil_sensor(mavlink_message_t *msg);
+	void handle_message_hil_gps(mavlink_message_t *msg);
+	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
+
+	void *receive_thread(void *arg);
+
+	mavlink_status_t status;
+	struct vehicle_local_position_s hil_local_pos;
+	int _manual_sub;
+	orb_advert_t _global_pos_pub;
+	orb_advert_t _local_pos_pub;
+	orb_advert_t _attitude_pub;
+	orb_advert_t _gps_pub;
+	orb_advert_t _sensors_pub;
+	orb_advert_t _gyro_pub;
+	orb_advert_t _accel_pub;
+	orb_advert_t _mag_pub;
+	orb_advert_t _baro_pub;
+	orb_advert_t _airspeed_pub;
+	orb_advert_t _battery_pub;
+	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;
+	orb_advert_t _rc_pub;
+	orb_advert_t _manual_pub;
+	int _hil_frames;
+	uint64_t _old_timestamp;
+	bool _hil_local_proj_inited;
+	float _hil_local_alt0;
+};

src/modules/mavlink/mavlink_stream.cpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_stream.cpp
+ * Mavlink messages stream implementation.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <stdlib.h>
+
+#include "mavlink_stream.h"
+#include "mavlink_main.h"
+
+MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr)
+{
+}
+
+MavlinkStream::~MavlinkStream()
+{
+}
+
+/**
+ * Set messages interval in ms
+ */
+void
+MavlinkStream::set_interval(const unsigned int interval)
+{
+	_interval = interval;
+}
+
+/**
+ * Set mavlink channel
+ */
+void
+MavlinkStream::set_channel(mavlink_channel_t channel)
+{
+	_channel = channel;
+}
+
+/**
+ * Update subscriptions and send message if necessary
+ */
+int
+MavlinkStream::update(const hrt_abstime t)
+{
+	uint64_t dt = t - _last_sent;
+
+	if (dt > 0 && dt >= _interval) {
+		/* interval expired, send message */
+		send(t);
+		_last_sent = (t / _interval) * _interval;
+	}
+}

src/modules/mavlink/mavlink_stream.h

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_stream.cpp
+ * Mavlink messages stream definition.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef MAVLINK_STREAM_H_
+#define MAVLINK_STREAM_H_
+
+#include <drivers/drv_hrt.h>
+
+class Mavlink;
+class MavlinkStream;
+
+#include "mavlink_main.h"
+
+class MavlinkStream
+{
+private:
+	hrt_abstime _last_sent;
+
+protected:
+	mavlink_channel_t _channel;
+	unsigned int _interval;
+
+	virtual void send(const hrt_abstime t) = 0;
+
+public:
+	MavlinkStream *next;
+
+	MavlinkStream();
+	~MavlinkStream();
+	void set_interval(const unsigned int interval);
+	void set_channel(mavlink_channel_t channel);
+	int update(const hrt_abstime t);
+	virtual MavlinkStream *new_instance() = 0;
+	virtual void subscribe(Mavlink *mavlink) = 0;
+	virtual const char *get_name() = 0;
+};
+
+
+#endif /* MAVLINK_STREAM_H_ */

src/modules/mavlink/module.mk

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -36,10 +36,12 @@
 #
 
 MODULE_COMMAND	 = mavlink
-SRCS		 += mavlink.c \
-			   mavlink_parameters.c \
-			   mavlink_receiver.cpp \
-			   orb_listener.c \
-			   waypoints.c
+SRCS		 += mavlink_main.cpp \
+			mavlink.c \
+			mavlink_receiver.cpp \
+			mavlink_orb_subscription.cpp \
+			mavlink_messages.cpp \
+			mavlink_stream.cpp \
+			mavlink_rate_limiter.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink

src/modules/mavlink/orb_listener.c

@@ -1,848 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file orb_listener.c
- * Monitors ORB topics and sends update messages as appropriate.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-// XXX trim includes
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <string.h>
-#include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <sys/prctl.h>
-#include <stdlib.h>
-#include <poll.h>
-#include <lib/geo/geo.h>
-
-#include <mavlink/mavlink_log.h>
-
-#include "waypoints.h"
-#include "orb_topics.h"
-#include "mavlink_hil.h"
-#include "util.h"
-
-extern bool gcs_link;
-
-struct vehicle_global_position_s global_pos;
-struct vehicle_local_position_s local_pos;
-struct home_position_s home;
-struct navigation_capabilities_s nav_cap;
-struct vehicle_status_s v_status;
-struct position_setpoint_triplet_s pos_sp_triplet;
-struct rc_channels_s rc;
-struct rc_input_values rc_raw;
-struct actuator_armed_s armed;
-struct actuator_controls_s actuators_0;
-struct vehicle_attitude_s att;
-struct airspeed_s airspeed;
-
-struct mavlink_subscriptions mavlink_subs;
-
-static int status_sub;
-static int rc_sub;
-
-static unsigned int sensors_raw_counter;
-static unsigned int attitude_counter;
-static unsigned int gps_counter;
-
-/*
- * Last sensor loop time
- * some outputs are better timestamped
- * with this "global" reference.
- */
-static uint64_t last_sensor_timestamp;
-
-static hrt_abstime last_sent_vfr = 0;
-
-static void	*uorb_receive_thread(void *arg);
-
-struct listener {
-	void	(*callback)(const struct listener *l);
-	int		*subp;
-	uintptr_t	arg;
-};
-
-uint16_t cm_uint16_from_m_float(float m);
-
-static void	l_sensor_combined(const struct listener *l);
-static void	l_vehicle_attitude(const struct listener *l);
-static void	l_vehicle_gps_position(const struct listener *l);
-static void	l_vehicle_status(const struct listener *l);
-static void	l_rc_channels(const struct listener *l);
-static void	l_input_rc(const struct listener *l);
-static void	l_global_position(const struct listener *l);
-static void	l_local_position(const struct listener *l);
-static void	l_global_position_setpoint(const struct listener *l);
-static void	l_local_position_setpoint(const struct listener *l);
-static void	l_attitude_setpoint(const struct listener *l);
-static void	l_actuator_outputs(const struct listener *l);
-static void	l_actuator_armed(const struct listener *l);
-static void	l_manual_control_setpoint(const struct listener *l);
-static void	l_vehicle_attitude_controls(const struct listener *l);
-static void	l_debug_key_value(const struct listener *l);
-static void	l_optical_flow(const struct listener *l);
-static void	l_vehicle_rates_setpoint(const struct listener *l);
-static void	l_home(const struct listener *l);
-static void	l_airspeed(const struct listener *l);
-static void	l_nav_cap(const struct listener *l);
-
-static const struct listener listeners[] = {
-	{l_sensor_combined,		&mavlink_subs.sensor_sub,	0},
-	{l_vehicle_attitude,		&mavlink_subs.att_sub,		0},
-	{l_vehicle_gps_position,	&mavlink_subs.gps_sub,		0},
-	{l_vehicle_status,		&status_sub,			0},
-	{l_rc_channels,			&rc_sub,			0},
-	{l_input_rc,			&mavlink_subs.input_rc_sub,	0},
-	{l_global_position,		&mavlink_subs.global_pos_sub,	0},
-	{l_local_position,		&mavlink_subs.local_pos_sub,	0},
-	{l_global_position_setpoint,	&mavlink_subs.triplet_sub,	0},
-	{l_local_position_setpoint,	&mavlink_subs.spl_sub,		0},
-	{l_attitude_setpoint,		&mavlink_subs.spa_sub,		0},
-	{l_actuator_outputs,		&mavlink_subs.act_0_sub,	0},
-	{l_actuator_outputs,		&mavlink_subs.act_1_sub,	1},
-	{l_actuator_outputs,		&mavlink_subs.act_2_sub,	2},
-	{l_actuator_outputs,		&mavlink_subs.act_3_sub,	3},
-	{l_actuator_armed,		&mavlink_subs.armed_sub,	0},
-	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
-	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
-	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
-	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
-	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
-	{l_home,			&mavlink_subs.home_sub,		0},
-	{l_airspeed,			&mavlink_subs.airspeed_sub,		0},
-	{l_nav_cap,			&mavlink_subs.navigation_capabilities_sub,		0},
-};
-
-static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
-
-uint16_t
-cm_uint16_from_m_float(float m)
-{
-	if (m < 0.0f) {
-		return 0;
-
-	} else if (m > 655.35f) {
-		return 65535;
-	}
-
-	return (uint16_t)(m * 100.0f);
-}
-
-void
-l_sensor_combined(const struct listener *l)
-{
-	struct sensor_combined_s raw;
-
-	/* copy sensors raw data into local buffer */
-	orb_copy(ORB_ID(sensor_combined), mavlink_subs.sensor_sub, &raw);
-
-	last_sensor_timestamp = raw.timestamp;
-
-	/* mark individual fields as changed */
-	uint16_t fields_updated = 0;
-	static unsigned accel_counter = 0;
-	static unsigned gyro_counter = 0;
-	static unsigned mag_counter = 0;
-	static unsigned baro_counter = 0;
-
-	if (accel_counter != raw.accelerometer_counter) {
-		/* mark first three dimensions as changed */
-		fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
-		accel_counter = raw.accelerometer_counter;
-	}
-
-	if (gyro_counter != raw.gyro_counter) {
-		/* mark second group dimensions as changed */
-		fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
-		gyro_counter = raw.gyro_counter;
-	}
-
-	if (mag_counter != raw.magnetometer_counter) {
-		/* mark third group dimensions as changed */
-		fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
-		mag_counter = raw.magnetometer_counter;
-	}
-
-	if (baro_counter != raw.baro_counter) {
-		/* mark last group dimensions as changed */
-		fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
-		baro_counter = raw.baro_counter;
-	}
-
-	if (gcs_link)
-		mavlink_msg_highres_imu_send(MAVLINK_COMM_0, last_sensor_timestamp,
-					     raw.accelerometer_m_s2[0], raw.accelerometer_m_s2[1],
-					     raw.accelerometer_m_s2[2], raw.gyro_rad_s[0],
-					     raw.gyro_rad_s[1], raw.gyro_rad_s[2],
-					     raw.magnetometer_ga[0],
-					     raw.magnetometer_ga[1], raw.magnetometer_ga[2],
-					     raw.baro_pres_mbar, raw.differential_pressure_pa,
-					     raw.baro_alt_meter, raw.baro_temp_celcius,
-					     fields_updated);
-
-	sensors_raw_counter++;
-}
-
-void
-l_vehicle_attitude(const struct listener *l)
-{
-	/* copy attitude data into local buffer */
-	orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
-
-	if (gcs_link) {
-		/* send sensor values */
-		mavlink_msg_attitude_send(MAVLINK_COMM_0,
-					  last_sensor_timestamp / 1000,
-					  att.roll,
-					  att.pitch,
-					  att.yaw,
-					  att.rollspeed,
-					  att.pitchspeed,
-					  att.yawspeed);
-					  	
-		/* limit VFR message rate to 10Hz */
-		hrt_abstime t = hrt_absolute_time();
-		if (t >= last_sent_vfr + 100000) {
-			last_sent_vfr = t;
-			float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
-			uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
-			float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
-			mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d);
-		}
-		
-		/* send quaternion values if it exists */
-		if(att.q_valid) {
-			mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
-												last_sensor_timestamp / 1000,
-												att.q[0],
-												att.q[1],
-												att.q[2],
-												att.q[3],
-												att.rollspeed,
-												att.pitchspeed,
-												att.yawspeed);
-		}
-	}
-
-	attitude_counter++;
-}
-
-void
-l_vehicle_gps_position(const struct listener *l)
-{
-	struct vehicle_gps_position_s gps;
-
-	/* copy gps data into local buffer */
-	orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
-
-	/* GPS COG is 0..2PI in degrees * 1e2 */
-	float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F;
-
-	/* GPS position */
-	mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
-				     gps.timestamp_position,
-				     gps.fix_type,
-				     gps.lat,
-				     gps.lon,
-				     gps.alt,
-				     cm_uint16_from_m_float(gps.eph_m),
-				     cm_uint16_from_m_float(gps.epv_m),
-				     gps.vel_m_s * 1e2f, // from m/s to cm/s
-				     cog_deg * 1e2f, // from deg to deg * 100
-				     gps.satellites_visible);
-
-	/* update SAT info every 10 seconds */
-	if (gps.satellite_info_available && (gps_counter % 50 == 0)) {
-		mavlink_msg_gps_status_send(MAVLINK_COMM_0,
-					    gps.satellites_visible,
-					    gps.satellite_prn,
-					    gps.satellite_used,
-					    gps.satellite_elevation,
-					    gps.satellite_azimuth,
-					    gps.satellite_snr);
-	}
-
-	gps_counter++;
-}
-
-void
-l_vehicle_status(const struct listener *l)
-{
-	/* immediately communicate state changes back to user */
-	orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
-	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
-	orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.position_setpoint_triplet_sub, &pos_sp_triplet);
-
-	/* enable or disable HIL */
-	if (v_status.hil_state == HIL_STATE_ON)
-		set_hil_on_off(true);
-	else if (v_status.hil_state == HIL_STATE_OFF)
-		set_hil_on_off(false);
-
-	/* translate the current syste state to mavlink state and mode */
-	uint8_t mavlink_state = 0;
-	uint8_t mavlink_base_mode = 0;
-	uint32_t mavlink_custom_mode = 0;
-	get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-
-	/* send heartbeat */
-	mavlink_msg_heartbeat_send(chan,
-				   mavlink_system.type,
-				   MAV_AUTOPILOT_PX4,
-				   mavlink_base_mode,
-				   mavlink_custom_mode,
-				   mavlink_state);
-}
-
-void
-l_rc_channels(const struct listener *l)
-{
-	/* copy rc channels into local buffer */
-	orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
-	// XXX Add RC channels scaled message here
-}
-
-void
-l_input_rc(const struct listener *l)
-{
-	/* copy rc channels into local buffer */
-	orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
-
-	if (gcs_link) {
-
-		const unsigned port_width = 8;
-
-		for (unsigned i = 0; (i * port_width) < rc_raw.channel_count; i++) {
-			/* Channels are sent in MAVLink main loop at a fixed interval */
-			mavlink_msg_rc_channels_raw_send(chan,
-							 rc_raw.timestamp_publication / 1000,
-							 i,
-							 (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX,
-							 (rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX,
-							 rc_raw.rssi);
-		}
-	}
-}
-
-void
-l_global_position(const struct listener *l)
-{
-	/* copy global position data into local buffer */
-	orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
-
-	mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
-						 global_pos.timestamp / 1000,
-					     global_pos.lat * 1e7,
-					     global_pos.lon * 1e7,
-					     global_pos.alt * 1000.0f,
-					     (global_pos.alt - home.alt) * 1000.0f,
-					     global_pos.vel_n * 100.0f,
-					     global_pos.vel_e * 100.0f,
-					     global_pos.vel_d * 100.0f,
-					     _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
-}
-
-void
-l_local_position(const struct listener *l)
-{
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos);
-
-	if (gcs_link)
-		mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
-						    local_pos.timestamp / 1000,
-						    local_pos.x,
-						    local_pos.y,
-						    local_pos.z,
-						    local_pos.vx,
-						    local_pos.vy,
-						    local_pos.vz);
-}
-
-void
-l_global_position_setpoint(const struct listener *l)
-{
-	struct position_setpoint_triplet_s triplet;
-	orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.triplet_sub, &triplet);
-
-	if (!triplet.current.valid)
-		return;
-
-	if (gcs_link)
-		mavlink_msg_global_position_setpoint_int_send(MAVLINK_COMM_0,
-				MAV_FRAME_GLOBAL,
-				(int32_t)(triplet.current.lat * 1e7d),
-				(int32_t)(triplet.current.lon * 1e7d),
-				(int32_t)(triplet.current.alt * 1e3f),
-				(int16_t)(triplet.current.yaw * M_RAD_TO_DEG_F * 1e2f));
-}
-
-void
-l_local_position_setpoint(const struct listener *l)
-{
-	struct vehicle_local_position_setpoint_s local_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_local_position_setpoint), mavlink_subs.spl_sub, &local_sp);
-
-	if (gcs_link)
-		mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0,
-				MAV_FRAME_LOCAL_NED,
-				local_sp.x,
-				local_sp.y,
-				local_sp.z,
-				local_sp.yaw);
-}
-
-void
-l_attitude_setpoint(const struct listener *l)
-{
-	struct vehicle_attitude_setpoint_s att_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_attitude_setpoint), mavlink_subs.spa_sub, &att_sp);
-
-	if (gcs_link)
-		mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0,
-				att_sp.timestamp / 1000,
-				att_sp.roll_body,
-				att_sp.pitch_body,
-				att_sp.yaw_body,
-				att_sp.thrust);
-}
-
-void
-l_vehicle_rates_setpoint(const struct listener *l)
-{
-	struct vehicle_rates_setpoint_s rates_sp;
-
-	/* copy local position data into local buffer */
-	orb_copy(ORB_ID(vehicle_rates_setpoint), mavlink_subs.rates_setpoint_sub, &rates_sp);
-
-	if (gcs_link)
-		mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(MAVLINK_COMM_0,
-				rates_sp.timestamp / 1000,
-				rates_sp.roll,
-				rates_sp.pitch,
-				rates_sp.yaw,
-				rates_sp.thrust);
-}
-
-void
-l_actuator_outputs(const struct listener *l)
-{
-	struct actuator_outputs_s act_outputs;
-
-	orb_id_t ids[] = {
-		ORB_ID(actuator_outputs_0),
-		ORB_ID(actuator_outputs_1),
-		ORB_ID(actuator_outputs_2),
-		ORB_ID(actuator_outputs_3)
-	};
-
-	/* copy actuator data into local buffer */
-	orb_copy(ids[l->arg], *l->subp, &act_outputs);
-
-	if (gcs_link) {
-		mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
-						  l->arg /* port number - needs GCS support */,
-							 /* QGC has port number support already */
-						  act_outputs.output[0],
-						  act_outputs.output[1],
-						  act_outputs.output[2],
-						  act_outputs.output[3],
-						  act_outputs.output[4],
-						  act_outputs.output[5],
-						  act_outputs.output[6],
-						  act_outputs.output[7]);
-
-		/* only send in HIL mode and only send first group for HIL */
-		if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
-
-			/* translate the current syste state to mavlink state and mode */
-			uint8_t mavlink_state = 0;
-			uint8_t mavlink_base_mode = 0;
-			uint32_t mavlink_custom_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-
-			/* HIL message as per MAVLink spec */
-
-			/* scale / assign outputs depending on system type */
-
-			if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
-				mavlink_msg_hil_controls_send(chan,
-							      hrt_absolute_time(),
-							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-							      -1,
-							      -1,
-							      -1,
-							      -1,
-							      mavlink_base_mode,
-							      0);
-
-			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
-				mavlink_msg_hil_controls_send(chan,
-							      hrt_absolute_time(),
-							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-							      -1,
-							      -1,
-							      mavlink_base_mode,
-							      0);
-
-			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
-				mavlink_msg_hil_controls_send(chan,
-							      hrt_absolute_time(),
-							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
-							      ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
-							      mavlink_base_mode,
-							      0);
-
-			} else {
-				mavlink_msg_hil_controls_send(chan,
-							      hrt_absolute_time(),
-							      (act_outputs.output[0] - 1500.0f) / 500.0f,
-							      (act_outputs.output[1] - 1500.0f) / 500.0f,
-							      (act_outputs.output[2] - 1500.0f) / 500.0f,
-							      (act_outputs.output[3] - 1000.0f) / 1000.0f,
-							      (act_outputs.output[4] - 1500.0f) / 500.0f,
-							      (act_outputs.output[5] - 1500.0f) / 500.0f,
-							      (act_outputs.output[6] - 1500.0f) / 500.0f,
-							      (act_outputs.output[7] - 1500.0f) / 500.0f,
-							      mavlink_base_mode,
-							      0);
-			}
-		}
-	}
-}
-
-void
-l_actuator_armed(const struct listener *l)
-{
-	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
-}
-
-void
-l_manual_control_setpoint(const struct listener *l)
-{
-	struct manual_control_setpoint_s man_control;
-
-	/* copy manual control data into local buffer */
-	orb_copy(ORB_ID(manual_control_setpoint), mavlink_subs.man_control_sp_sub, &man_control);
-
-	if (gcs_link)
-		mavlink_msg_manual_control_send(MAVLINK_COMM_0,
-						mavlink_system.sysid,
-						man_control.roll * 1000,
-						man_control.pitch * 1000,
-						man_control.yaw * 1000,
-						man_control.throttle * 1000,
-						0);
-}
-
-void
-l_vehicle_attitude_controls(const struct listener *l)
-{
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
-
-	if (gcs_link) {
-		/* send, add spaces so that string buffer is at least 10 chars long */
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "ctrl0    ",
-						   actuators_0.control[0]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "ctrl1    ",
-						   actuators_0.control[1]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "ctrl2     ",
-						   actuators_0.control[2]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "ctrl3     ",
-						   actuators_0.control[3]);
-	}
-}
-
-void
-l_debug_key_value(const struct listener *l)
-{
-	struct debug_key_value_s debug;
-
-	orb_copy(ORB_ID(debug_key_value), mavlink_subs.debug_key_value, &debug);
-
-	/* Enforce null termination */
-	debug.key[sizeof(debug.key) - 1] = '\0';
-
-	mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-					   last_sensor_timestamp / 1000,
-					   debug.key,
-					   debug.value);
-}
-
-void
-l_optical_flow(const struct listener *l)
-{
-	struct optical_flow_s flow;
-
-	orb_copy(ORB_ID(optical_flow), mavlink_subs.optical_flow, &flow);
-
-	mavlink_msg_optical_flow_send(MAVLINK_COMM_0, flow.timestamp, flow.sensor_id, flow.flow_raw_x, flow.flow_raw_y,
-				      flow.flow_comp_x_m, flow.flow_comp_y_m, flow.quality, flow.ground_distance_m);
-}
-
-void
-l_home(const struct listener *l)
-{
-	orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
-
-	mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f);
-}
-
-void
-l_airspeed(const struct listener *l)
-{
-	orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
-}
-
-void
-l_nav_cap(const struct listener *l)
-{
-
-	orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap);
-
-	mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-				   hrt_absolute_time() / 1000,
-				   "turn dist",
-				   nav_cap.turn_distance);
-
-}
-
-static void *
-uorb_receive_thread(void *arg)
-{
-	/* Set thread name */
-	prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid());
-
-	/*
-	 * set up poll to block for new data,
-	 * wait for a maximum of 1000 ms (1 second)
-	 */
-	const int timeout = 1000;
-
-	/*
-	 * Initialise listener array.
-	 *
-	 * Might want to invoke each listener once to set initial state.
-	 */
-	struct pollfd fds[n_listeners];
-
-	for (unsigned i = 0; i < n_listeners; i++) {
-		fds[i].fd = *listeners[i].subp;
-		fds[i].events = POLLIN;
-
-		/* Invoke callback to set initial state */
-		//listeners[i].callback(&listener[i]);
-	}
-
-	while (!thread_should_exit) {
-
-		int poll_ret = poll(fds, n_listeners, timeout);
-
-		/* handle the poll result */
-		if (poll_ret == 0) {
-			/* silent */
-
-		} else if (poll_ret < 0) {
-			mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
-
-		} else {
-
-			for (unsigned i = 0; i < n_listeners; i++) {
-				if (fds[i].revents & POLLIN)
-					listeners[i].callback(&listeners[i]);
-			}
-		}
-	}
-
-	return NULL;
-}
-
-pthread_t
-uorb_receive_start(void)
-{
-	/* --- SENSORS RAW VALUE --- */
-	mavlink_subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	/* rate limit set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink_subs.sensor_sub, 200);	/* 5Hz updates */
-
-	/* --- ATTITUDE VALUE --- */
-	mavlink_subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	/* rate limit set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink_subs.att_sub, 200);	/* 5Hz updates */
-
-	/* --- GPS VALUE --- */
-	mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	orb_set_interval(mavlink_subs.gps_sub, 200);	/* 5Hz updates */
-
-	/* --- HOME POSITION --- */
-	mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position));
-	orb_set_interval(mavlink_subs.home_sub, 1000);	/* 1Hz updates */
-
-	/* --- SYSTEM STATE --- */
-	status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	orb_set_interval(status_sub, 300);		/* max 3.33 Hz updates */
-
-	/* --- POSITION SETPOINT TRIPLET --- */
-	mavlink_subs.position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
-	orb_set_interval(mavlink_subs.position_setpoint_triplet_sub, 0);		/* not polled, don't limit */
-
-	/* --- RC CHANNELS VALUE --- */
-	rc_sub = orb_subscribe(ORB_ID(rc_channels));
-	orb_set_interval(rc_sub, 100);			/* 10Hz updates */
-
-	/* --- RC RAW VALUE --- */
-	mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc));
-	orb_set_interval(mavlink_subs.input_rc_sub, 100);
-
-	/* --- GLOBAL POS VALUE --- */
-	mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	orb_set_interval(mavlink_subs.global_pos_sub, 100);	/* 10 Hz active updates */
-
-	/* --- LOCAL POS VALUE --- */
-	mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	orb_set_interval(mavlink_subs.local_pos_sub, 1000);	/* 1Hz active updates */
-
-	/* --- GLOBAL SETPOINT VALUE --- */
-	mavlink_subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
-	orb_set_interval(mavlink_subs.triplet_sub, 2000);	/* 0.5 Hz updates */
-	
-	/* --- LOCAL SETPOINT VALUE --- */
-	mavlink_subs.spl_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
-	orb_set_interval(mavlink_subs.spl_sub, 2000);	/* 0.5 Hz updates */
-
-	/* --- ATTITUDE SETPOINT VALUE --- */
-	mavlink_subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	orb_set_interval(mavlink_subs.spa_sub, 2000);	/* 0.5 Hz updates */
-
-	/* --- RATES SETPOINT VALUE --- */
-	mavlink_subs.rates_setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	orb_set_interval(mavlink_subs.rates_setpoint_sub, 2000);  /* 0.5 Hz updates */
-
-	/* --- ACTUATOR OUTPUTS --- */
-	mavlink_subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
-	mavlink_subs.act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
-	mavlink_subs.act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
-	mavlink_subs.act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
-	/* rate limits set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink_subs.act_0_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink_subs.act_1_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink_subs.act_2_sub, 100);	/* 10Hz updates */
-	orb_set_interval(mavlink_subs.act_3_sub, 100);	/* 10Hz updates */
-
-	/* --- ACTUATOR ARMED VALUE --- */
-	mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(mavlink_subs.armed_sub, 100);	/* 10Hz updates */
-
-	/* --- MAPPED MANUAL CONTROL INPUTS --- */
-	mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	/* rate limits set externally based on interface speed, set a basic default here */
-	orb_set_interval(mavlink_subs.man_control_sp_sub, 100);	/* 10Hz updates */
-
-	/* --- ACTUATOR CONTROL VALUE --- */
-	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */
-
-	/* --- DEBUG VALUE OUTPUT --- */
-	mavlink_subs.debug_key_value = orb_subscribe(ORB_ID(debug_key_value));
-	orb_set_interval(mavlink_subs.debug_key_value, 100);	/* 10Hz updates */
-
-	/* --- FLOW SENSOR --- */
-	mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
-	orb_set_interval(mavlink_subs.optical_flow, 200); 	/* 5Hz updates */
-
-	/* --- AIRSPEED --- */
-	mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
-	orb_set_interval(mavlink_subs.airspeed_sub, 200); 	/* 5Hz updates */
-
-	/* --- NAVIGATION CAPABILITIES --- */
-	mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
-	orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); 	/* 2Hz updates */
-	nav_cap.turn_distance = 0.0f;
-
-	/* start the listener loop */
-	pthread_attr_t uorb_attr;
-	pthread_attr_init(&uorb_attr);
-
-	/* Set stack size, needs less than 2k */
-	pthread_attr_setstacksize(&uorb_attr, 2048);
-
-	pthread_t thread;
-	pthread_create(&thread, &uorb_attr, uorb_receive_thread, NULL);
-
-	pthread_attr_destroy(&uorb_attr);
-	return thread;
-}

src/modules/mavlink/util.h

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/mavlink/waypoints.c

@@ -1,730 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file waypoints.c
- * MAVLink waypoint protocol implementation (BSD-relicensed).
- */
-
-#include <math.h>
-#include <sys/prctl.h>
-#include <unistd.h>
-#include <stdio.h>
-#include "mavlink_bridge_header.h"
-#include "waypoints.h"
-#include "util.h"
-#include <uORB/uORB.h>
-#include <uORB/topics/mission.h>
-#include <geo/geo.h>
-#include <dataman/dataman.h>
-#include <drivers/drv_hrt.h>
-#include <systemlib/err.h>
-
-bool verbose = true;
-
-orb_advert_t mission_pub = -1;
-struct mission_s mission;
-
-uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
-
-void
-mavlink_missionlib_send_message(mavlink_message_t *msg)
-{
-	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
-
-	mavlink_send_uart_bytes(chan, missionlib_msg_buf, len);
-}
-
-
-
-int
-mavlink_missionlib_send_gcs_string(const char *string)
-{
-	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
-	mavlink_statustext_t statustext;
-	int i = 0;
-
-	while (i < len - 1) {
-		statustext.text[i] = string[i];
-
-		if (string[i] == '\0')
-			break;
-
-		i++;
-	}
-
-	if (i > 1) {
-		/* Enforce null termination */
-		statustext.text[i] = '\0';
-		mavlink_message_t msg;
-
-		mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
-		mavlink_missionlib_send_message(&msg);
-		return OK;
-
-	} else {
-		return 1;
-	}
-}
-
-void publish_mission()
-{
-	/* Initialize mission publication if necessary */
-	if (mission_pub < 0) {
-		mission_pub = orb_advertise(ORB_ID(mission), &mission);
-
-	} else {
-		orb_publish(ORB_ID(mission), mission_pub, &mission);
-	}
-}
-
-int 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) {
-		case MAV_FRAME_GLOBAL:
-			mission_item->lat = (double)mavlink_mission_item->x;
-			mission_item->lon = (double)mavlink_mission_item->y;
-			mission_item->altitude = mavlink_mission_item->z;
-			mission_item->altitude_is_relative = false;
-			break;
-
-		case MAV_FRAME_GLOBAL_RELATIVE_ALT:
-			mission_item->lat = (double)mavlink_mission_item->x;
-			mission_item->lon = (double)mavlink_mission_item->y;
-			mission_item->altitude = mavlink_mission_item->z;
-			mission_item->altitude_is_relative = true;
-			break;
-
-		case MAV_FRAME_LOCAL_NED:
-		case MAV_FRAME_LOCAL_ENU:
-			return MAV_MISSION_UNSUPPORTED_FRAME;
-		case MAV_FRAME_MISSION:
-		default:
-			return MAV_MISSION_ERROR;
-	}
-
-	switch (mavlink_mission_item->command) {
-		case MAV_CMD_NAV_TAKEOFF:
-			mission_item->pitch_min = mavlink_mission_item->param2;
-			break;
-		default:
-			mission_item->acceptance_radius = mavlink_mission_item->param2;
-			break;
-	}
-
-	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->time_inside = mavlink_mission_item->param1;
-	mission_item->autocontinue = mavlink_mission_item->autocontinue;
-	// mission_item->index = mavlink_mission_item->seq;
-	mission_item->origin = ORIGIN_MAVLINK;
-
-	return OK;
-}
-
-int 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;
-	} else {
-		mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
-	}
-	
-	switch (mission_item->nav_cmd) {
-		case NAV_CMD_TAKEOFF:
-			mavlink_mission_item->param2 = mission_item->pitch_min;
-			break;
-		default:
-			mavlink_mission_item->param2 = mission_item->acceptance_radius;
-			break;
-	}
-
-	mavlink_mission_item->x = (float)mission_item->lat;
-	mavlink_mission_item->y = (float)mission_item->lon;
-	mavlink_mission_item->z = mission_item->altitude;
-
-	mavlink_mission_item->param4 = mission_item->yaw*M_RAD_TO_DEG_F;
-	mavlink_mission_item->param3 = mission_item->loiter_radius*(float)mission_item->loiter_direction;
-	mavlink_mission_item->command = mission_item->nav_cmd;
-	mavlink_mission_item->param1 = mission_item->time_inside;
-	mavlink_mission_item->autocontinue = mission_item->autocontinue;
-	// mavlink_mission_item->seq = mission_item->index;
-
-	return OK;
-}
-
-void mavlink_wpm_init(mavlink_wpm_storage *state)
-{
-	state->size = 0;
-	state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
-	state->current_state = MAVLINK_WPM_STATE_IDLE;
-	state->current_partner_sysid = 0;
-	state->current_partner_compid = 0;
-	state->timestamp_lastaction = 0;
-	state->timestamp_last_send_setpoint = 0;
-	state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
-	state->current_dataman_id = 0;
-}
-
-/*
- *  @brief Sends an waypoint ack message
- */
-void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
-{
-	mavlink_message_t msg;
-	mavlink_mission_ack_t wpa;
-
-	wpa.target_system = sysid;
-	wpa.target_component = compid;
-	wpa.type = type;
-
-	mavlink_msg_mission_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa);
-	mavlink_missionlib_send_message(&msg);
-
-	if (verbose) warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system);
-}
-
-/*
- *  @brief Broadcasts the new target waypoint and directs the MAV to fly there
- *
- *  This function broadcasts its new active waypoint sequence number and
- *  sends a message to the controller, advising it to fly to the coordinates
- *  of the waypoint with a given orientation
- *
- *  @param seq The waypoint sequence number the MAV should fly to.
- */
-void mavlink_wpm_send_waypoint_current(uint16_t seq)
-{
-	if (seq < wpm->size) {
-		mavlink_message_t msg;
-		mavlink_mission_current_t wpc;
-
-		wpc.seq = seq;
-
-		mavlink_msg_mission_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
-		mavlink_missionlib_send_message(&msg);
-
-		if (verbose) warnx("Broadcasted new current waypoint %u", wpc.seq);
-
-	} else {
-		mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
-		if (verbose) warnx("ERROR: index out of bounds");
-	}
-}
-
-void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count)
-{
-	mavlink_message_t msg;
-	mavlink_mission_count_t wpc;
-
-	wpc.target_system = sysid;
-	wpc.target_component = compid;
-	wpc.count = mission.count;
-
-	mavlink_msg_mission_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc);
-	mavlink_missionlib_send_message(&msg);
-
-	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)
-{
-
-	struct mission_item_s mission_item;
-	ssize_t len = sizeof(struct mission_item_s);
-	
-	dm_item_t dm_current;
-
-	if (wpm->current_dataman_id == 0) {
-		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
-	} else {
-		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
-	}
-
-	if (dm_read(dm_current, seq, &mission_item, len) == len) {
-
-		/* create mission_item_s from mavlink_mission_item_t */
-		mavlink_mission_item_t wp;
-		map_mission_item_to_mavlink_mission_item(&mission_item, &wp);
-
-		mavlink_message_t msg;
-		wp.target_system = sysid;
-		wp.target_component = compid;
-		wp.seq = seq;
-		mavlink_msg_mission_item_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp);
-		mavlink_missionlib_send_message(&msg);
-
-		if (verbose) warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system);
-	} else {
-		mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
-		if (verbose) warnx("ERROR: could not read WP%u", seq);
-	}
-}
-
-void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
-{
-	if (seq < wpm->max_size) {
-		mavlink_message_t msg;
-		mavlink_mission_request_t wpr;
-		wpr.target_system = sysid;
-		wpr.target_component = compid;
-		wpr.seq = seq;
-		mavlink_msg_mission_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr);
-		mavlink_missionlib_send_message(&msg);
-
-		if (verbose) warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system);
-
-	} else {
-		mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
-		if (verbose) warnx("ERROR: Waypoint index exceeds list capacity");
-	}
-}
-
-/*
- *  @brief emits a message that a waypoint reached
- *
- *  This function broadcasts a message that a waypoint is reached.
- *
- *  @param seq The waypoint sequence number the MAV has reached.
- */
-void mavlink_wpm_send_waypoint_reached(uint16_t seq)
-{
-	mavlink_message_t msg;
-	mavlink_mission_item_reached_t wp_reached;
-
-	wp_reached.seq = seq;
-
-	mavlink_msg_mission_item_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached);
-	mavlink_missionlib_send_message(&msg);
-
-	if (verbose) warnx("Sent waypoint %u reached message", wp_reached.seq);
-}
-
-
-void 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) {
-
-		mavlink_missionlib_send_gcs_string("Operation timeout");
-
-		if (verbose) warnx("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_state);
-
-		wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-		wpm->current_partner_sysid = 0;
-		wpm->current_partner_compid = 0;
-	}
-}
-
-
-void mavlink_wpm_message_handler(const mavlink_message_t *msg)
-{
-	uint64_t now = hrt_absolute_time();
-
-	switch (msg->msgid) {
-
-		case MAVLINK_MSG_ID_MISSION_ACK: {
-			mavlink_mission_ack_t wpa;
-			mavlink_msg_mission_ack_decode(msg, &wpa);
-
-			if ((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) {
-				wpm->timestamp_lastaction = now;
-
-				if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
-					if (wpm->current_wp_id == wpm->size - 1) {
-
-						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-						wpm->current_wp_id = 0;
-					}
-				}
-
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
-				if (verbose) warnx("REJ. WP CMD: curr partner id mismatch");
-			}
-
-			break;
-		}
-
-		case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
-			mavlink_mission_set_current_t wpc;
-			mavlink_msg_mission_set_current_decode(msg, &wpc);
-
-			if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
-				wpm->timestamp_lastaction = now;
-
-				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-					if (wpc.seq < wpm->size) {
-
-						mission.current_index = wpc.seq;
-						publish_mission();
-						
-						mavlink_wpm_send_waypoint_current(wpc.seq);
-
-					} else {
-						mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
-						if (verbose) warnx("IGN WP CURR CMD: Not in list");
-					}
-
-				} else {
-					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
-					if (verbose) warnx("IGN WP CURR CMD: Busy");
-
-				}
-
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-				if (verbose) warnx("REJ. WP CMD: target id mismatch");
-			}
-
-			break;
-		}
-
-		case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
-			mavlink_mission_request_list_t wprl;
-			mavlink_msg_mission_request_list_decode(msg, &wprl);
-
-			if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
-				wpm->timestamp_lastaction = now;
-
-				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
-					if (wpm->size > 0) {
-						
-						wpm->current_state = MAVLINK_WPM_STATE_SENDLIST;
-						wpm->current_wp_id = 0;
-						wpm->current_partner_sysid = msg->sysid;
-						wpm->current_partner_compid = msg->compid;
-
-					} else {
-						if (verbose) warnx("No waypoints send");
-					}
-
-					wpm->current_count = wpm->size;
-					mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count);
-
-				} else {
-					mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
-					if (verbose) warnx("IGN REQUEST LIST: Busy");
-				}
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch");
-				if (verbose) warnx("REJ. REQUEST LIST: target id mismatch");
-			}
-
-			break;
-		}
-
-		case MAVLINK_MSG_ID_MISSION_REQUEST: {
-			mavlink_mission_request_t wpr;
-			mavlink_msg_mission_request_decode(msg, &wpr);
-
-			if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
-				wpm->timestamp_lastaction = now;
-
-				if (wpr.seq >= wpm->size) {
-
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
-					if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.", wpr.seq);
-					break;
-				}
-
-				/* 
-				 * Ensure that we are in the correct state and that the first request has id 0 
-				 * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
-				 */
-				if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
-
-					if (wpr.seq == 0) {
-						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
-						wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
-					} else {
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
-						if (verbose) warnx("REJ. WP CMD: First id != 0");
-						break;
-					}
-
-				} else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
-
-					if (wpr.seq == wpm->current_wp_id) {
-
-						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
-
-					} else if (wpr.seq == wpm->current_wp_id + 1) {
-
-						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid);
-					
-					} else {
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
-						if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1);
-						break;
-					}
-
-				} else {
-
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-					if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).", wpm->current_state);
-					break;
-				}
-
-				wpm->current_wp_id = wpr.seq;
-				wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
-
-				if (wpr.seq < wpm->size) {
-
-					mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid,wpm->current_wp_id);
-
-				} else {
-					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
-					if (verbose) warnx("ERROR: Waypoint %u out of bounds", wpr.seq);
-				}
-
-
-			} else {
-				//we we're target but already communicating with someone else
-				if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) {
-
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-					if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, wpm->current_partner_sysid);
-
-				} else {
-
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-					if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
-				}
-			}
-			break;
-		}
-
-		case MAVLINK_MSG_ID_MISSION_COUNT: {
-			mavlink_mission_count_t wpc;
-			mavlink_msg_mission_count_decode(msg, &wpc);
-
-			if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
-				wpm->timestamp_lastaction = now;
-
-				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-
-					if (wpc.count > NUM_MISSIONS_SUPPORTED) {
-						if (verbose) warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
-						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_NO_SPACE);
-						break;
-					}
-
-					if (wpc.count == 0) {
-						mavlink_missionlib_send_gcs_string("COUNT 0");
-						if (verbose) warnx("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE");
-						break;
-					}
-					
-					if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid);
-
-					wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
-					wpm->current_wp_id = 0;
-					wpm->current_partner_sysid = msg->sysid;
-					wpm->current_partner_compid = msg->compid;
-					wpm->current_count = wpc.count;
-
-					mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
-
-				} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
-
-					if (wpm->current_wp_id == 0) {
-						mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
-						if (verbose) warnx("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u", wpc.count, msg->sysid);
-					} else {
-						mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
-						if (verbose) warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.", wpm->current_wp_id);
-					}
-				} else {
-						mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
-						if (verbose) warnx("IGN MISSION_COUNT CMD: Busy");
-				}
-			} else {
-
-				mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch");
-				if (verbose) warnx("IGNORED WAYPOINT COUNT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
-			}
-		}
-		break;
-
-		case MAVLINK_MSG_ID_MISSION_ITEM: {
-			mavlink_mission_item_t wp;
-			mavlink_msg_mission_item_decode(msg, &wp);
-
-			if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) {
-
-				wpm->timestamp_lastaction = now;
-
-				/*
-				 * ensure that we are in the correct state and that the first waypoint has id 0
-				 * and the following waypoints have the correct ids
-				 */
-
-				if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
-
-				 	if (wp.seq != 0) {
-				 		mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
-				 		warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.", wp.seq);
-				 		break;
-				 	}
-				} else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-
-					if (wp.seq >= wpm->current_count) {
-						mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds");
-						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.", wp.seq);
-						break;
-					}
-
-					if (wp.seq != wpm->current_wp_id) {
-						warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.", wp.seq, wpm->current_wp_id);
-						mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
-						break;
-					}
-				}
-
-				wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
-
-				struct mission_item_s mission_item;
-
-				int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item);
-
-				if (ret != OK) {
-					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret);
-					wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-					break;
-				}
-
-				ssize_t len = sizeof(struct mission_item_s);
-
-				dm_item_t dm_next;
-
-				if (wpm->current_dataman_id == 0) {
-					dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1;
-					mission.dataman_id = 1;
-				} else {
-					dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0;
-					mission.dataman_id = 0;
-				}
-
-				if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
-					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
-					wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-					break;
-				}
-
-				if (wp.current) {
-					mission.current_index = wp.seq;
-				}
-
-				wpm->current_wp_id = wp.seq + 1;
-
-				if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
-					
-					if (verbose) warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", wpm->current_count);
-
-					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
-
-					mission.count = wpm->current_count;
-					
-					publish_mission();
-
-					wpm->current_dataman_id = mission.dataman_id;
-					wpm->size = wpm->current_count;
-
-					wpm->current_state = MAVLINK_WPM_STATE_IDLE;
-
-				} else {
-					mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id);
-				}
-
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
-				if (verbose) warnx("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
-			}
-
-			break;
-		}
-
-		case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
-			mavlink_mission_clear_all_t wpca;
-			mavlink_msg_mission_clear_all_decode(msg, &wpca);
-
-			if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
-
-				if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
-					wpm->timestamp_lastaction = now;
-
-					wpm->size = 0;
-
-					/* prepare mission topic */
-					mission.dataman_id = -1;
-					mission.count = 0;
-					mission.current_index = -1;
-					publish_mission();
-
-					if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) {
-						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
-					} else {
-						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
-					}
-
-					
-				} else {
-					mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy");
-					if (verbose) warnx("IGN WP CLEAR CMD: Busy");
-				}
-
-
-			} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
-
-				mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch");
-				if (verbose) warnx("IGNORED WAYPOINT CLEAR COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH");
-			}
-
-			break;
-		}
-
-	default: {
-			/* other messages might should get caught by mavlink and others */
-			break;
-		}
-	}
-}

src/modules/mavlink/waypoints.h

@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2009-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,11 @@
 /**
  * @file waypoints.h
  * MAVLink waypoint protocol definition (BSD-relicensed).
+ *
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef WAYPOINTS_H_
@@ -106,7 +108,4 @@ extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float pa
 
 static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
 
-void mavlink_missionlib_send_message(mavlink_message_t *msg);
-int mavlink_missionlib_send_gcs_string(const char *string);
-
 #endif /* WAYPOINTS_H_ */

src/modules/mavlink_onboard/mavlink.c

@@ -1,537 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file mavlink.c
- * MAVLink 1.0 protocol implementation.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <mqueue.h>
-#include <string.h>
-#include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <poll.h>
-
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/err.h>
-
-#include "orb_topics.h"
-#include "util.h"
-
-__EXPORT int mavlink_onboard_main(int argc, char *argv[]);
-
-static int mavlink_thread_main(int argc, char *argv[]);
-
-/* thread state */
-volatile bool thread_should_exit = false;
-static volatile bool thread_running = false;
-static int mavlink_task;
-
-/* pthreads */
-static pthread_t receive_thread;
-
-/* terminate MAVLink on user request - disabled by default */
-static bool mavlink_link_termination_allowed = false;
-
-mavlink_system_t mavlink_system = {
-	100,
-	50,
-	MAV_TYPE_QUADROTOR,
-	0,
-	0,
-	0
-}; // System ID, 1-255, Component/Subsystem ID, 1-255
-
-/* XXX not widely used */
-uint8_t chan = MAVLINK_COMM_0;
-
-/* XXX probably should be in a header... */
-extern pthread_t receive_start(int uart);
-
-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 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);
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
-{
-	/* process baud rate */
-	int speed;
-
-	switch (baud) {
-	case 0:      speed = B0;      break;
-
-	case 50:     speed = B50;     break;
-
-	case 75:     speed = B75;     break;
-
-	case 110:    speed = B110;    break;
-
-	case 134:    speed = B134;    break;
-
-	case 150:    speed = B150;    break;
-
-	case 200:    speed = B200;    break;
-
-	case 300:    speed = B300;    break;
-
-	case 600:    speed = B600;    break;
-
-	case 1200:   speed = B1200;   break;
-
-	case 1800:   speed = B1800;   break;
-
-	case 2400:   speed = B2400;   break;
-
-	case 4800:   speed = B4800;   break;
-
-	case 9600:   speed = B9600;   break;
-
-	case 19200:  speed = B19200;  break;
-
-	case 38400:  speed = B38400;  break;
-
-	case 57600:  speed = B57600;  break;
-
-	case 115200: speed = B115200; break;
-
-	case 230400: speed = B230400; break;
-
-	case 460800: speed = B460800; break;
-
-	case 921600: speed = B921600; break;
-
-	default:
-		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600", baud);
-		return -EINVAL;
-	}
-
-	/* open uart */
-	warnx("UART is %s, baudrate is %d", uart_name, baud);
-	uart = open(uart_name, O_RDWR | O_NOCTTY);
-
-	/* Try to set baud rate */
-	struct termios uart_config;
-	int termios_state;
-	*is_usb = false;
-
-	if (strcmp(uart_name, "/dev/ttyACM0") != OK) {
-		/* Back up the original uart configuration to restore it after exit */
-		if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-			warnx("ERROR getting baudrate / termios config for %s: %d", uart_name, termios_state);
-			close(uart);
-			return -1;
-		}
-
-		/* Fill the struct for the new configuration */
-		tcgetattr(uart, &uart_config);
-
-		/* Clear ONLCR flag (which appends a CR for every LF) */
-		uart_config.c_oflag &= ~ONLCR;
-
-		/* Set baud rate */
-		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-			warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", uart_name, termios_state);
-			close(uart);
-			return -1;
-		}
-
-
-		if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-			warnx("ERROR setting baudrate / termios config for %s (tcsetattr)", uart_name);
-			close(uart);
-			return -1;
-		}
-
-	} else {
-		*is_usb = true;
-	}
-
-	return uart;
-}
-
-void 
-mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
-{
-	write(uart, ch, (size_t)(sizeof(uint8_t) * length));
-}
-
-/*
- * Internal function to give access to the channel status for each channel
- */
-mavlink_status_t* mavlink_get_channel_status(uint8_t channel)
-{
-	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
-	return &m_mavlink_status[channel];
-}
-
-/*
- * Internal function to give access to the channel buffer for each channel
- */
-mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel)
-{
-	static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
-	return &m_mavlink_buffer[channel];
-}
-
-void mavlink_update_system(void)
-{
-	static bool initialized = false;
-	param_t param_system_id;
-	param_t param_component_id;
-	param_t param_system_type;
-
-	if (!initialized) {
-		param_system_id = param_find("MAV_SYS_ID");
-		param_component_id = param_find("MAV_COMP_ID");
-		param_system_type = param_find("MAV_TYPE");
-	}
-
-	/* update system and component id */
-	int32_t system_id;
-	param_get(param_system_id, &system_id);
-	if (system_id > 0 && system_id < 255) {
-		mavlink_system.sysid = system_id;
-	}
-
-	int32_t component_id;
-	param_get(param_component_id, &component_id);
-	if (component_id > 0 && component_id < 255) {
-		mavlink_system.compid = component_id;
-	}
-	
-	int32_t system_type;
-	param_get(param_system_type, &system_type);
-	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
-		mavlink_system.type = system_type;
-	}
-}
-
-void
-get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
-	uint8_t *mavlink_state, uint8_t *mavlink_mode)
-{
-	/* reset MAVLink mode bitfield */
-	*mavlink_mode = 0;
-
-	/* set mode flags independent of system state */
-	if (control_mode->flag_control_manual_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-	}
-
-	if (control_mode->flag_system_hil_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
-	}
-
-	/* set arming state */
-	if (armed->armed) {
-		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-	} else {
-		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
-	}
-
-	if (control_mode->flag_control_velocity_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-	} else {
-		*mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED;
-	}
-
-//	switch (v_status->state_machine) {
-//	case SYSTEM_STATE_PREFLIGHT:
-//		if (v_status->flag_preflight_gyro_calibration ||
-//		    v_status->flag_preflight_mag_calibration ||
-//		    v_status->flag_preflight_accel_calibration) {
-//			*mavlink_state = MAV_STATE_CALIBRATING;
-//		} else {
-//			*mavlink_state = MAV_STATE_UNINIT;
-//		}
-//		break;
-//
-//	case SYSTEM_STATE_STANDBY:
-//		*mavlink_state = MAV_STATE_STANDBY;
-//		break;
-//
-//	case SYSTEM_STATE_GROUND_READY:
-//		*mavlink_state = MAV_STATE_ACTIVE;
-//		break;
-//
-//	case SYSTEM_STATE_MANUAL:
-//		*mavlink_state = MAV_STATE_ACTIVE;
-//		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-//		break;
-//
-//	case SYSTEM_STATE_STABILIZED:
-//		*mavlink_state = MAV_STATE_ACTIVE;
-//		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-//		break;
-//
-//	case SYSTEM_STATE_AUTO:
-//		*mavlink_state = MAV_STATE_ACTIVE;
-//		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-//		break;
-//
-//	case SYSTEM_STATE_MISSION_ABORT:
-//		*mavlink_state = MAV_STATE_EMERGENCY;
-//		break;
-//
-//	case SYSTEM_STATE_EMCY_LANDING:
-//		*mavlink_state = MAV_STATE_EMERGENCY;
-//		break;
-//
-//	case SYSTEM_STATE_EMCY_CUTOFF:
-//		*mavlink_state = MAV_STATE_EMERGENCY;
-//		break;
-//
-//	case SYSTEM_STATE_GROUND_ERROR:
-//		*mavlink_state = MAV_STATE_EMERGENCY;
-//		break;
-//
-//	case SYSTEM_STATE_REBOOT:
-//		*mavlink_state = MAV_STATE_POWEROFF;
-//		break;
-//	}
-
-}
-
-/**
- * MAVLink Protocol main function.
- */
-int mavlink_thread_main(int argc, char *argv[])
-{
-	int ch;
-	char *device_name = "/dev/ttyS1";
-	baudrate = 57600;
-
-	/* XXX this is never written? */
-	struct vehicle_status_s v_status;
-	struct vehicle_control_mode_s control_mode;
-	struct actuator_armed_s armed;
-
-	/* work around some stupidity in task_create's argv handling */
-	argc -= 2;
-	argv += 2;
-
-	while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
-		switch (ch) {
-		case 'b':
-			baudrate = strtoul(optarg, NULL, 10);
-			if (baudrate == 0)
-				errx(1, "invalid baud rate '%s'", optarg);
-			break;
-
-		case 'd':
-			device_name = optarg;
-			break;
-
-		case 'e':
-			mavlink_link_termination_allowed = true;
-			break;
-
-		case 'o':
-			mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
-			break;
-
-		default:
-			usage();
-		}
-	}
-
-	struct termios uart_config_original;
-	bool usb_uart;
-
-	/* print welcome text */
-	warnx("MAVLink v1.0 serial interface starting...");
-
-	/* inform about mode */
-	warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
-
-	/* Flush stdout in case MAVLink is about to take it over */
-	fflush(stdout);
-
-	/* default values for arguments */
-	uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
-	if (uart < 0)
-		err(1, "could not open %s", device_name);
-
-	/* Initialize system properties */
-	mavlink_update_system();
-
-	/* start the MAVLink receiver */
-	receive_thread = receive_start(uart);
-
-	thread_running = true;
-
-	/* arm counter to go off immediately */
-	unsigned lowspeed_counter = 10;
-
-	while (!thread_should_exit) {
-
-		/* 1 Hz */
-		if (lowspeed_counter == 10) {
-			mavlink_update_system();
-
-			/* translate the current system state to mavlink state and mode */
-			uint8_t mavlink_state = 0;
-			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode);
-
-			/* send heartbeat */
-			// TODO fix navigation state, use control_mode topic
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, 0, mavlink_state);
-
-			/* send status (values already copied in the section above) */
-			mavlink_msg_sys_status_send(chan,
-						    v_status.onboard_control_sensors_present,
-						    v_status.onboard_control_sensors_enabled,
-						    v_status.onboard_control_sensors_health,
-						    v_status.load * 1000.0f,
-						    v_status.battery_voltage * 1000.0f,
-						    v_status.battery_current * 1000.0f,
-						    v_status.battery_remaining,
-						    v_status.drop_rate_comm,
-						    v_status.errors_comm,
-						    v_status.errors_count1,
-						    v_status.errors_count2,
-						    v_status.errors_count3,
-						    v_status.errors_count4);
-			lowspeed_counter = 0;
-		}
-		lowspeed_counter++;
-
-		/* sleep 1000 ms */
-		usleep(1000000);
-	}
-
-	/* wait for threads to complete */
-	pthread_join(receive_thread, NULL);
-
-	/* Reset the UART flags to original state */
-	if (!usb_uart)
-		tcsetattr(uart, TCSANOW, &uart_config_original);
-
-	thread_running = false;
-
-	exit(0);
-}
-
-static void
-usage()
-{
-	fprintf(stderr, "usage: mavlink_onboard start [-d <devicename>] [-b <baud rate>]\n"
-			"       mavlink_onboard stop\n"
-			"       mavlink_onboard status\n");
-	exit(1);
-}
-
-int mavlink_onboard_main(int argc, char *argv[])
-{
-
-	if (argc < 2) {
-		warnx("missing command");
-		usage();
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		/* this is not an error */
-		if (thread_running)
-			errx(0, "already running");
-
-		thread_should_exit = false;
-		mavlink_task = task_spawn_cmd("mavlink_onboard",
-					  SCHED_DEFAULT,
-					  SCHED_PRIORITY_DEFAULT,
-					  2048,
-					  mavlink_thread_main,
-					  (const char**)argv);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		while (thread_running) {
-			usleep(200000);
-		}
-		warnx("terminated");
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			errx(0, "running");
-		} else {
-			errx(1, "not running");
-		}
-	}
-
-	warnx("unrecognized command");
-	usage();
-	/* not getting here */
-	return 0;
-}
-

src/modules/mavlink_onboard/mavlink_receiver.c

@@ -1,344 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file mavlink_receiver.c
- * MAVLink protocol message receive and dispatch
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-/* XXX trim includes */
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <mqueue.h>
-#include <string.h>
-#include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <poll.h>
-
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-
-#include "util.h"
-#include "orb_topics.h"
-
-/* XXX should be in a header somewhere */
-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_attitude_s hil_attitude;
-orb_advert_t pub_hil_global_pos = -1;
-orb_advert_t pub_hil_attitude = -1;
-
-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;
-
-extern bool gcs_link;
-
-static void
-handle_message(mavlink_message_t *msg)
-{
-	if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
-
-		mavlink_command_long_t cmd_mavlink;
-		mavlink_msg_command_long_decode(msg, &cmd_mavlink);
-
-		if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
-				|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
-			//check for MAVLINK terminate command
-			if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
-				/* This is the link shutdown command, terminate mavlink */
-				warnx("terminating...");
-				fflush(stdout);
-				usleep(50000);
-
-				/* terminate other threads and this thread */
-				thread_should_exit = true;
-
-			} else {
-
-				/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
-				vcmd.param1 = cmd_mavlink.param1;
-				vcmd.param2 = cmd_mavlink.param2;
-				vcmd.param3 = cmd_mavlink.param3;
-				vcmd.param4 = cmd_mavlink.param4;
-				vcmd.param5 = cmd_mavlink.param5;
-				vcmd.param6 = cmd_mavlink.param6;
-				vcmd.param7 = cmd_mavlink.param7;
-				vcmd.command = cmd_mavlink.command;
-				vcmd.target_system = cmd_mavlink.target_system;
-				vcmd.target_component = cmd_mavlink.target_component;
-				vcmd.source_system = msg->sysid;
-				vcmd.source_component = msg->compid;
-				vcmd.confirmation =  cmd_mavlink.confirmation;
-
-				/* check if topic is advertised */
-				if (cmd_pub <= 0) {
-					cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
-				}
-
-				/* publish */
-				orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
-			}
-		}
-	}
-
-	if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
-		mavlink_optical_flow_t flow;
-		mavlink_msg_optical_flow_decode(msg, &flow);
-
-		struct optical_flow_s f;
-
-		f.timestamp = hrt_absolute_time();
-		f.flow_raw_x = flow.flow_x;
-		f.flow_raw_y = flow.flow_y;
-		f.flow_comp_x_m = flow.flow_comp_m_x;
-		f.flow_comp_y_m = flow.flow_comp_m_y;
-		f.ground_distance_m = flow.ground_distance;
-		f.quality = flow.quality;
-		f.sensor_id = flow.sensor_id;
-
-		/* check if topic is advertised */
-		if (flow_pub <= 0) {
-			flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
-
-		} else {
-			/* publish */
-			orb_publish(ORB_ID(optical_flow), flow_pub, &f);
-		}
-
-	}
-
-	if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
-		/* Set mode on request */
-		mavlink_set_mode_t new_mode;
-		mavlink_msg_set_mode_decode(msg, &new_mode);
-
-		/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
-		vcmd.param1 = new_mode.base_mode;
-		vcmd.param2 = new_mode.custom_mode;
-		vcmd.param3 = 0;
-		vcmd.param4 = 0;
-		vcmd.param5 = 0;
-		vcmd.param6 = 0;
-		vcmd.param7 = 0;
-		vcmd.command = MAV_CMD_DO_SET_MODE;
-		vcmd.target_system = new_mode.target_system;
-		vcmd.target_component = MAV_COMP_ID_ALL;
-		vcmd.source_system = msg->sysid;
-		vcmd.source_component = msg->compid;
-		vcmd.confirmation = 1;
-
-		/* check if topic is advertised */
-		if (cmd_pub <= 0) {
-			cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
-
-		} else {
-			/* create command */
-			orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
-		}
-	}
-
-	/* Handle Vicon position estimates */
-	if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
-		mavlink_vicon_position_estimate_t pos;
-		mavlink_msg_vicon_position_estimate_decode(msg, &pos);
-
-		vicon_position.x = pos.x;
-		vicon_position.y = pos.y;
-		vicon_position.z = pos.z;
-
-		if (vicon_position_pub <= 0) {
-			vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
-
-		} else {
-			orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
-		}
-	}
-
-	/* Handle quadrotor motor setpoints */
-
-	if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
-		mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
-		mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
-
-		if (mavlink_system.sysid < 4) {
-
-			/* switch to a receiving link mode */
-			gcs_link = false;
-
-			/*
-			 * rate control mode - defined by MAVLink
-			 */
-
-			uint8_t ml_mode = 0;
-			bool ml_armed = false;
-
-			switch (quad_motors_setpoint.mode) {
-			case 0:
-				ml_armed = false;
-				break;
-
-			case 1:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
-				ml_armed = true;
-
-				break;
-
-			case 2:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
-				ml_armed = true;
-
-				break;
-
-			case 3:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
-				break;
-
-			case 4:
-				ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
-				break;
-			}
-
-			offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1]   / (float)INT16_MAX;
-			offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1]  / (float)INT16_MAX;
-			offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1]    / (float)INT16_MAX;
-			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
-
-			if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
-				ml_armed = false;
-			}
-
-			offboard_control_sp.armed = ml_armed;
-			offboard_control_sp.mode = ml_mode;
-
-			offboard_control_sp.timestamp = hrt_absolute_time();
-
-			/* check if topic has to be advertised */
-			if (offboard_control_sp_pub <= 0) {
-				offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
-
-			} else {
-				/* Publish */
-				orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
-			}
-		}
-	}
-
-}
-
-
-/**
- * Receive data from UART.
- */
-static void *
-receive_thread(void *arg)
-{
-	int uart_fd = *((int *)arg);
-
-	const int timeout = 1000;
-	uint8_t buf[32];
-
-	mavlink_message_t msg;
-
-	prctl(PR_SET_NAME, "mavlink_onboard_rcv", getpid());
-
-	struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
-
-	ssize_t nread = 0;
-
-	while (!thread_should_exit) {
-		if (poll(fds, 1, timeout) > 0) {
-			if (nread < sizeof(buf)) {
-				/* to avoid reading very small chunks wait for data before reading */
-				usleep(1000);
-			}
-
-			/* non-blocking read. read may return negative values */
-			nread = read(uart_fd, buf, sizeof(buf));
-
-			/* if read failed, this loop won't execute */
-			for (ssize_t i = 0; i < nread; i++) {
-				if (mavlink_parse_char(chan, buf[i], &msg, &status)) {
-					/* handle generic messages and commands */
-					handle_message(&msg);
-				}
-			}
-		}
-	}
-
-	return NULL;
-}
-
-pthread_t
-receive_start(int uart)
-{
-	pthread_attr_t receiveloop_attr;
-	pthread_attr_init(&receiveloop_attr);
-
-	struct sched_param param;
-	param.sched_priority = SCHED_PRIORITY_MAX - 40;
-	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
-
-	pthread_attr_setstacksize(&receiveloop_attr, 2048);
-
-	pthread_t thread;
-	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
-	return thread;
-}

src/modules/mavlink_onboard/module.mk

@@ -1,42 +0,0 @@
-############################################################################
-#
-#   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# MAVLink protocol to uORB interface process (XXX hack for onboard use)
-#
-
-MODULE_COMMAND	 = mavlink_onboard
-SRCS		 = mavlink.c \
-		   mavlink_receiver.c
-
-INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink

src/modules/mavlink_onboard/orb_topics.h

@@ -1,102 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file orb_topics.h
- * Common sets of topics subscribed to or published by the MAVLink driver,
- * and structures maintained by those subscriptions.
- */
-#pragma once
-
-#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_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/vehicle_vicon_position.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_attitude_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.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/debug_key_value.h>
-#include <drivers/drv_rc_input.h>
-
-struct mavlink_subscriptions {
-	int sensor_sub;
-	int att_sub;
-	int global_pos_sub;
-	int act_0_sub;
-	int act_1_sub;
-	int act_2_sub;
-	int act_3_sub;
-	int gps_sub;
-	int man_control_sp_sub;
-	int safety_sub;
-	int actuators_sub;
-	int local_pos_sub;
-	int spa_sub;
-	int spl_sub;
-	int spg_sub;
-	int debug_key_value;
-	int input_rc_sub;
-};
-
-extern struct mavlink_subscriptions mavlink_subs;
-
-/** Global position */
-extern struct vehicle_global_position_s global_pos;
-
-/** Local position */
-extern struct vehicle_local_position_s local_pos;
-
-/** Vehicle status */
-// extern struct vehicle_status_s v_status;
-
-/** RC channels */
-extern struct rc_channels_s rc;
-
-/** Actuator armed state */
-// extern struct actuator_armed_s armed;
-
-/** Worker thread starter */
-extern pthread_t uorb_receive_start(void);

src/modules/navigator/navigator_main.cpp

@@ -65,7 +65,6 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/mission_result.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
@@ -288,9 +287,9 @@ private:
 	static void	task_main_trampoline(int argc, char *argv[]);
 
 	/**
-	 * Main sensor collection task.
+	 * Main task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 
 	void		publish_safepoints(unsigned points);
 
@@ -395,7 +394,6 @@ Navigator::Navigator() :
 
 /* publications */
 	_pos_sp_triplet_pub(-1),
-	_mission_result_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -427,7 +425,6 @@ Navigator::Navigator() :
 	_parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T");
 
 	memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s));
-	memset(&_mission_result, 0, sizeof(struct mission_result_s));
 	memset(&_mission_item, 0, sizeof(struct mission_item_s));
 
 	memset(&nav_states_str, 0, sizeof(nav_states_str));
@@ -529,13 +526,16 @@ Navigator::offboard_mission_update(bool isrotaryWing)
 		missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence);
 
 		_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
-		_mission.set_current_offboard_mission_index(offboard_mission.current_index);
+
 		_mission.set_offboard_mission_count(offboard_mission.count);
+		_mission.set_current_offboard_mission_index(offboard_mission.current_index);
 
 	} else {
-		_mission.set_current_offboard_mission_index(0);
 		_mission.set_offboard_mission_count(0);
+		_mission.set_current_offboard_mission_index(0);
 	}
+
+	_mission.publish_mission_result();
 }
 
 void
@@ -545,12 +545,12 @@ Navigator::onboard_mission_update()
 
 	if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
 
-		_mission.set_current_onboard_mission_index(onboard_mission.current_index);
 		_mission.set_onboard_mission_count(onboard_mission.count);
+		_mission.set_current_onboard_mission_index(onboard_mission.current_index);
 
 	} else {
-		_mission.set_current_onboard_mission_index(0);
 		_mission.set_onboard_mission_count(0);
+		_mission.set_current_onboard_mission_index(0);
 	}
 }
 
@@ -1112,6 +1112,8 @@ Navigator::set_mission_item()
 	ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
 
 	if (ret == OK) {
+		_mission.report_current_offboard_mission_item();
+
 		_mission_item_valid = true;
 		position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
 
@@ -1581,6 +1583,9 @@ void
 Navigator::on_mission_item_reached()
 {
 	if (myState == NAV_STATE_MISSION) {
+
+		_mission.report_mission_item_reached();
+
 		if (_do_takeoff) {
 			/* takeoff completed */
 			_do_takeoff = false;
@@ -1627,6 +1632,7 @@ Navigator::on_mission_item_reached()
 		mavlink_log_info(_mavlink_fd, "[navigator] landing completed");
 		dispatch(EVENT_READY_REQUESTED);
 	}
+	_mission.publish_mission_result();
 }
 
 void

src/modules/navigator/navigator_mission.cpp

@@ -36,13 +36,12 @@
  * Helper class to access missions
  */
 
-// #include <stdio.h>
-// #include <stdlib.h>
-// #include <string.h>
-// #include <unistd.h>
-
+#include <string.h>
 #include <stdlib.h>
 #include <dataman/dataman.h>
+#include <systemlib/err.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/mission_result.h>
 #include "navigator_mission.h"
 
 /* oddly, ERROR is not defined for c++ */
@@ -60,8 +59,11 @@ Mission::Mission() :
 	_offboard_mission_item_count(0),
 	_onboard_mission_item_count(0),
 	_onboard_mission_allowed(false),
-	_current_mission_type(MISSION_TYPE_NONE)
-{}
+	_current_mission_type(MISSION_TYPE_NONE),
+	_mission_result_pub(-1)
+{
+	memset(&_mission_result, 0, sizeof(struct mission_result_s));
+}
 
 Mission::~Mission()
 {
@@ -78,8 +80,16 @@ void
 Mission::set_current_offboard_mission_index(int new_index)
 {
 	if (new_index != -1) {
+		warnx("specifically set to %d", new_index);
 		_current_offboard_mission_index = (unsigned)new_index;
+	} else {
+
+		/* if less WPs available, reset to first WP */
+		if (_current_offboard_mission_index >= _offboard_mission_item_count) {
+			_current_offboard_mission_index = 0;
+		}
 	}
+	report_current_offboard_mission_item();
 }
 
 void
@@ -87,7 +97,15 @@ Mission::set_current_onboard_mission_index(int new_index)
 {
 	if (new_index != -1) {
 		_current_onboard_mission_index = (unsigned)new_index;
+	} else {
+
+		/* if less WPs available, reset to first WP */
+		if (_current_onboard_mission_index >= _onboard_mission_item_count) {
+			_current_onboard_mission_index = 0;
+		}
 	}
+	// TODO: implement this for onboard missions as well
+	// report_current_mission_item();
 }
 
 void
@@ -266,4 +284,35 @@ Mission::move_to_next()
 	default:
 		break;
 	}
-}
+}
+
+void
+Mission::report_mission_item_reached()
+{
+	if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
+		_mission_result.mission_reached = true;
+		_mission_result.mission_index_reached = _current_offboard_mission_index;
+	}
+}
+
+void
+Mission::report_current_offboard_mission_item()
+{
+	_mission_result.index_current_mission = _current_offboard_mission_index;
+}
+
+void
+Mission::publish_mission_result()
+{
+	/* lazily publish the mission result only once available */
+	if (_mission_result_pub > 0) {
+		/* publish mission result */
+		orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
+
+	} else {
+		/* advertise and publish */
+		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
+	}
+	/* reset reached bool */
+	_mission_result.mission_reached = false;
+}

src/modules/navigator/navigator_mission.h

@@ -40,6 +40,7 @@
 #define NAVIGATOR_MISSION_H
 
 #include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
 
 
 class __EXPORT Mission
@@ -71,7 +72,9 @@ public:
 
 	void		move_to_next();
 
-	void		add_home_pos(struct mission_item_s *new_mission_item);
+	void		report_mission_item_reached();
+	void		report_current_offboard_mission_item();
+	void		publish_mission_result();
 
 private:
 	bool		current_onboard_mission_available();
@@ -92,6 +95,10 @@ private:
 		MISSION_TYPE_ONBOARD,
 		MISSION_TYPE_OFFBOARD,
 	} 		_current_mission_type;
+
+	int		_mission_result_pub;
+
+	struct mission_result_s _mission_result;
 };
 
-#endif
+#endif

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -200,8 +200,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	bool landed = true;
 	hrt_abstime landed_time = 0;
 
-	uint32_t accel_counter = 0;
-	uint32_t baro_counter = 0;
+	hrt_abstime accel_timestamp = 0;
+	hrt_abstime baro_timestamp = 0;
 
 	bool ref_inited = false;
 	hrt_abstime ref_init_start = 0;
@@ -310,8 +310,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (fds_init[0].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-				if (wait_baro && sensor.baro_counter != baro_counter) {
-					baro_counter = sensor.baro_counter;
+				if (wait_baro && sensor.baro_timestamp != baro_timestamp) {
+					baro_timestamp = sensor.baro_timestamp;
 
 					/* mean calculation over several measurements */
 					if (baro_init_cnt < baro_init_num) {
@@ -384,7 +384,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (updated) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-				if (sensor.accelerometer_counter != accel_counter) {
+				if (sensor.accelerometer_timestamp != accel_timestamp) {
 					if (att.R_valid) {
 						/* correct accel bias */
 						sensor.accelerometer_m_s2[0] -= acc_bias[0];
@@ -408,13 +408,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						memset(corr_acc, 0, sizeof(corr_acc));
 					}
 
-					accel_counter = sensor.accelerometer_counter;
+					accel_timestamp = sensor.accelerometer_timestamp;
 					accel_updates++;
 				}
 
-				if (sensor.baro_counter != baro_counter) {
+				if (sensor.baro_timestamp != baro_timestamp) {
 					corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
-					baro_counter = sensor.baro_counter;
+					baro_timestamp = sensor.baro_timestamp;
 					baro_updates++;
 				}
 			}

src/modules/sdlog2/sdlog2.c

@@ -887,11 +887,11 @@ int sdlog2_thread_main(int argc, char *argv[])
 	pthread_cond_init(&logbuffer_cond, NULL);
 
 	/* track changes in sensor_combined topic */
-	uint16_t gyro_counter = 0;
-	uint16_t accelerometer_counter = 0;
-	uint16_t magnetometer_counter = 0;
-	uint16_t baro_counter = 0;
-	uint16_t differential_pressure_counter = 0;
+	hrt_abstime gyro_timestamp = 0;
+	hrt_abstime accelerometer_timestamp = 0;
+	hrt_abstime magnetometer_timestamp = 0;
+	hrt_abstime barometer_timestamp = 0;
+	hrt_abstime differential_pressure_timestamp = 0;
 
 	/* track changes in distance status */
 	bool dist_bottom_present = false;
@@ -976,28 +976,28 @@ int sdlog2_thread_main(int argc, char *argv[])
 			bool write_IMU = false;
 			bool write_SENS = false;
 
-			if (buf.sensor.gyro_counter != gyro_counter) {
-				gyro_counter = buf.sensor.gyro_counter;
+			if (buf.sensor.timestamp != gyro_timestamp) {
+				gyro_timestamp = buf.sensor.timestamp;
 				write_IMU = true;
 			}
 
-			if (buf.sensor.accelerometer_counter != accelerometer_counter) {
-				accelerometer_counter = buf.sensor.accelerometer_counter;
+			if (buf.sensor.accelerometer_timestamp != accelerometer_timestamp) {
+				accelerometer_timestamp = buf.sensor.accelerometer_timestamp;
 				write_IMU = true;
 			}
 
-			if (buf.sensor.magnetometer_counter != magnetometer_counter) {
-				magnetometer_counter = buf.sensor.magnetometer_counter;
+			if (buf.sensor.magnetometer_timestamp != magnetometer_timestamp) {
+				magnetometer_timestamp = buf.sensor.magnetometer_timestamp;
 				write_IMU = true;
 			}
 
-			if (buf.sensor.baro_counter != baro_counter) {
-				baro_counter = buf.sensor.baro_counter;
+			if (buf.sensor.baro_timestamp != barometer_timestamp) {
+				barometer_timestamp = buf.sensor.baro_timestamp;
 				write_SENS = true;
 			}
 
-			if (buf.sensor.differential_pressure_counter != differential_pressure_counter) {
-				differential_pressure_counter = buf.sensor.differential_pressure_counter;
+			if (buf.sensor.differential_pressure_timestamp != differential_pressure_timestamp) {
+				differential_pressure_timestamp = buf.sensor.differential_pressure_timestamp;
 				write_SENS = true;
 			}
 
@@ -1023,6 +1023,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa;
 				LOGBUFFER_WRITE_AND_COUNT(SENS);
 			}
+
 		}
 
 		/* --- ATTITUDE --- */

src/modules/sensors/sensors.cpp

@@ -940,7 +940,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 		raw.accelerometer_raw[1] = accel_report.y_raw;
 		raw.accelerometer_raw[2] = accel_report.z_raw;
 
-		raw.accelerometer_counter++;
+		raw.accelerometer_timestamp = accel_report.timestamp;
 	}
 }
 
@@ -966,7 +966,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 		raw.gyro_raw[1] = gyro_report.y_raw;
 		raw.gyro_raw[2] = gyro_report.z_raw;
 
-		raw.gyro_counter++;
+		raw.timestamp = gyro_report.timestamp;
 	}
 }
 
@@ -996,7 +996,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 		raw.magnetometer_raw[1] = mag_report.y_raw;
 		raw.magnetometer_raw[2] = mag_report.z_raw;
 
-		raw.magnetometer_counter++;
+		raw.magnetometer_timestamp = mag_report.timestamp;
 	}
 }
 
@@ -1014,7 +1014,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 		raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
 		raw.baro_temp_celcius = _barometer.temperature; // Temperature in degrees celcius
 
-		raw.baro_counter++;
+		raw.baro_timestamp = _barometer.timestamp;
 	}
 }
 
@@ -1028,7 +1028,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres);
 
 		raw.differential_pressure_pa = _diff_pres.differential_pressure_pa;
-		raw.differential_pressure_counter++;
+		raw.differential_pressure_timestamp = _diff_pres.timestamp;
 
 		_airspeed.timestamp = hrt_absolute_time();
 		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa);
@@ -1593,8 +1593,7 @@ Sensors::task_main()
 		/* check parameters for updates */
 		parameter_update_poll();
 
-		/* store the time closest to all measurements (this is bogus, sensor timestamps should be propagated...) */
-		raw.timestamp = hrt_absolute_time();
+		/* the timestamp of the raw struct is updated by the gyro_poll() method */
 
 		/* copy most recent sensor data */
 		gyro_poll(raw);

src/modules/uORB/topics/mission_result.h

@@ -54,7 +54,8 @@
 struct mission_result_s
 {
 	bool mission_reached;		/**< true if mission has been reached			*/
-	unsigned mission_index;		/**< index of the mission which has been reached	*/
+	unsigned mission_index_reached;	/**< index of the mission which has been reached	*/
+	unsigned index_current_mission; /**< index of the current mission			*/
 };
 
 /**

src/modules/uORB/topics/sensor_combined.h

@@ -77,34 +77,33 @@ struct sensor_combined_s {
 
 	/* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only   */
 
-	uint64_t timestamp;			/**< Timestamp in microseconds since boot         */
+	uint64_t timestamp;			/**< Timestamp in microseconds since boot, from gyro         */
 
 	int16_t	gyro_raw[3];			/**< Raw sensor values of angular velocity        */
-	uint16_t gyro_counter;			/**< Number of raw measurments taken              */
 	float gyro_rad_s[3];			/**< Angular velocity in radian per seconds       */
-	
+
 	int16_t accelerometer_raw[3];		/**< Raw acceleration in NED body frame           */
-	uint32_t accelerometer_counter;		/**< Number of raw acc measurements taken         */
 	float accelerometer_m_s2[3];		/**< Acceleration in NED body frame, in m/s^2     */
 	int accelerometer_mode;			/**< Accelerometer measurement mode */
 	float accelerometer_range_m_s2;		/**< Accelerometer measurement range in m/s^2 */
+	uint64_t accelerometer_timestamp;	/**< Accelerometer timestamp        */
 
 	int16_t	magnetometer_raw[3];		/**< Raw magnetic field in NED body frame         */
 	float magnetometer_ga[3];		/**< Magnetic field in NED body frame, in Gauss   */
 	int magnetometer_mode;			/**< Magnetometer measurement mode */
 	float magnetometer_range_ga;		/**< ± measurement range in Gauss */
 	float magnetometer_cuttoff_freq_hz;	/**< Internal analog low pass frequency of sensor */
-	uint32_t magnetometer_counter;		/**< Number of raw mag measurements taken         */
-	
+	uint64_t magnetometer_timestamp;	/**< Magnetometer timestamp         */
+
 	float baro_pres_mbar;			/**< Barometric pressure, already temp. comp.     */
 	float baro_alt_meter;			/**< Altitude, already temp. comp.                */
 	float baro_temp_celcius;		/**< Temperature in degrees celsius               */
 	float adc_voltage_v[4];			/**< ADC voltages of ADC Chan 10/11/12/13 or -1      */
 	float mcu_temp_celcius;			/**< Internal temperature measurement of MCU */
-	uint32_t baro_counter;			/**< Number of raw baro measurements taken        */
+	uint64_t baro_timestamp;		/**< Barometer timestamp        */
 
-	float differential_pressure_pa;				/**< Airspeed sensor differential pressure                  */ 
-	uint32_t differential_pressure_counter;		/**< Number of raw differential pressure measurements taken */
+	float differential_pressure_pa;				/**< Airspeed sensor differential pressure                  */
+	uint64_t differential_pressure_timestamp;		/**< Last measurement timestamp */
 };
 
 /**

src/modules/uORB/topics/vehicle_attitude.h

@@ -76,7 +76,7 @@ struct vehicle_attitude_s {
 	float rate_offsets[3];	/**< Offsets of the body angular rates from zero		*/
 	float R[3][3];		/**< Rotation matrix body to world, (Tait-Bryan, NED)		*/
 	float q[4];		/**< Quaternion (NED)						*/
-	float g_comp[3];	/**< Compensated gravity vector				*/
+	float g_comp[3];	/**< Compensated gravity vector					*/
 	bool R_valid;		/**< Rotation matrix valid					*/
 	bool q_valid;		/**< Quaternion valid						*/