diff --git a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h
index 4d13e0b543c9a021cd9811f4378bb84dfdc47d49..64827032d4512c8bc54eaebf643402449e77c8f6 100644
--- a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h
+++ b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h
@@ -1,7 +1,7 @@
 /** @file
  *	@brief MAVLink comm protocol.
  *	@see http://qgroundcontrol.org/mavlink/
- *	 Generated on Wednesday, July 27 2011, 14:17 UTC
+ *	 Generated on Saturday, August 13 2011, 08:44 UTC
  */
 #ifndef ARDUPILOTMEGA_H
 #define ARDUPILOTMEGA_H
@@ -33,12 +33,14 @@ extern "C" {
 
 // MESSAGE DEFINITIONS
 
+#include "./mavlink_msg_sensor_offsets.h"
+#include "./mavlink_msg_set_mag_offsets.h"
 
 
 // MESSAGE LENGTHS
 
 #undef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
+#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
 
 #ifdef __cplusplus
 }
diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink.h b/thirdParty/mavlink/include/ardupilotmega/mavlink.h
index 8ee2430d74ef6945bd88749e49e4bc4105c40747..a54a6df31be8cf0a3c3ab7dfd8e2d3ca7b14855c 100644
--- a/thirdParty/mavlink/include/ardupilotmega/mavlink.h
+++ b/thirdParty/mavlink/include/ardupilotmega/mavlink.h
@@ -1,7 +1,7 @@
 /** @file
  *	@brief MAVLink comm protocol.
  *	@see http://pixhawk.ethz.ch/software/mavlink
- *	 Generated on Wednesday, July 27 2011, 14:17 UTC
+ *	 Generated on Saturday, August 13 2011, 08:44 UTC
  */
 #ifndef MAVLINK_H
 #define MAVLINK_H
diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h
new file mode 100644
index 0000000000000000000000000000000000000000..a3191c787073e5e7ff962713befe89a8b07bf7a2
--- /dev/null
+++ b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_sensor_offsets.h
@@ -0,0 +1,342 @@
+// MESSAGE SENSOR_OFFSETS PACKING
+
+#define MAVLINK_MSG_ID_SENSOR_OFFSETS 150
+
+typedef struct __mavlink_sensor_offsets_t 
+{
+	int16_t mag_ofs_x; ///< magnetometer X offset
+	int16_t mag_ofs_y; ///< magnetometer Y offset
+	int16_t mag_ofs_z; ///< magnetometer Z offset
+	float mag_declination; ///< magnetic declination (radians)
+	int32_t raw_press; ///< raw pressure from barometer
+	int32_t raw_temp; ///< raw temperature from barometer
+	float gyro_cal_x; ///< gyro X calibration
+	float gyro_cal_y; ///< gyro Y calibration
+	float gyro_cal_z; ///< gyro Z calibration
+	float accel_cal_x; ///< accel X calibration
+	float accel_cal_y; ///< accel Y calibration
+	float accel_cal_z; ///< accel Z calibration
+
+} mavlink_sensor_offsets_t;
+
+
+
+/**
+ * @brief Pack a sensor_offsets message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param mag_ofs_x magnetometer X offset
+ * @param mag_ofs_y magnetometer Y offset
+ * @param mag_ofs_z magnetometer Z offset
+ * @param mag_declination magnetic declination (radians)
+ * @param raw_press raw pressure from barometer
+ * @param raw_temp raw temperature from barometer
+ * @param gyro_cal_x gyro X calibration
+ * @param gyro_cal_y gyro Y calibration
+ * @param gyro_cal_z gyro Z calibration
+ * @param accel_cal_x accel X calibration
+ * @param accel_cal_y accel Y calibration
+ * @param accel_cal_z accel Z calibration
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_sensor_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS;
+
+	i += put_int16_t_by_index(mag_ofs_x, i, msg->payload); // magnetometer X offset
+	i += put_int16_t_by_index(mag_ofs_y, i, msg->payload); // magnetometer Y offset
+	i += put_int16_t_by_index(mag_ofs_z, i, msg->payload); // magnetometer Z offset
+	i += put_float_by_index(mag_declination, i, msg->payload); // magnetic declination (radians)
+	i += put_int32_t_by_index(raw_press, i, msg->payload); // raw pressure from barometer
+	i += put_int32_t_by_index(raw_temp, i, msg->payload); // raw temperature from barometer
+	i += put_float_by_index(gyro_cal_x, i, msg->payload); // gyro X calibration
+	i += put_float_by_index(gyro_cal_y, i, msg->payload); // gyro Y calibration
+	i += put_float_by_index(gyro_cal_z, i, msg->payload); // gyro Z calibration
+	i += put_float_by_index(accel_cal_x, i, msg->payload); // accel X calibration
+	i += put_float_by_index(accel_cal_y, i, msg->payload); // accel Y calibration
+	i += put_float_by_index(accel_cal_z, i, msg->payload); // accel Z calibration
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a sensor_offsets message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param mag_ofs_x magnetometer X offset
+ * @param mag_ofs_y magnetometer Y offset
+ * @param mag_ofs_z magnetometer Z offset
+ * @param mag_declination magnetic declination (radians)
+ * @param raw_press raw pressure from barometer
+ * @param raw_temp raw temperature from barometer
+ * @param gyro_cal_x gyro X calibration
+ * @param gyro_cal_y gyro Y calibration
+ * @param gyro_cal_z gyro Z calibration
+ * @param accel_cal_x accel X calibration
+ * @param accel_cal_y accel Y calibration
+ * @param accel_cal_z accel Z calibration
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_sensor_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_SENSOR_OFFSETS;
+
+	i += put_int16_t_by_index(mag_ofs_x, i, msg->payload); // magnetometer X offset
+	i += put_int16_t_by_index(mag_ofs_y, i, msg->payload); // magnetometer Y offset
+	i += put_int16_t_by_index(mag_ofs_z, i, msg->payload); // magnetometer Z offset
+	i += put_float_by_index(mag_declination, i, msg->payload); // magnetic declination (radians)
+	i += put_int32_t_by_index(raw_press, i, msg->payload); // raw pressure from barometer
+	i += put_int32_t_by_index(raw_temp, i, msg->payload); // raw temperature from barometer
+	i += put_float_by_index(gyro_cal_x, i, msg->payload); // gyro X calibration
+	i += put_float_by_index(gyro_cal_y, i, msg->payload); // gyro Y calibration
+	i += put_float_by_index(gyro_cal_z, i, msg->payload); // gyro Z calibration
+	i += put_float_by_index(accel_cal_x, i, msg->payload); // accel X calibration
+	i += put_float_by_index(accel_cal_y, i, msg->payload); // accel Y calibration
+	i += put_float_by_index(accel_cal_z, i, msg->payload); // accel Z calibration
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a sensor_offsets struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param sensor_offsets C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_sensor_offsets_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sensor_offsets_t* sensor_offsets)
+{
+	return mavlink_msg_sensor_offsets_pack(system_id, component_id, msg, sensor_offsets->mag_ofs_x, sensor_offsets->mag_ofs_y, sensor_offsets->mag_ofs_z, sensor_offsets->mag_declination, sensor_offsets->raw_press, sensor_offsets->raw_temp, sensor_offsets->gyro_cal_x, sensor_offsets->gyro_cal_y, sensor_offsets->gyro_cal_z, sensor_offsets->accel_cal_x, sensor_offsets->accel_cal_y, sensor_offsets->accel_cal_z);
+}
+
+/**
+ * @brief Send a sensor_offsets message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param mag_ofs_x magnetometer X offset
+ * @param mag_ofs_y magnetometer Y offset
+ * @param mag_ofs_z magnetometer Z offset
+ * @param mag_declination magnetic declination (radians)
+ * @param raw_press raw pressure from barometer
+ * @param raw_temp raw temperature from barometer
+ * @param gyro_cal_x gyro X calibration
+ * @param gyro_cal_y gyro Y calibration
+ * @param gyro_cal_z gyro Z calibration
+ * @param accel_cal_x accel X calibration
+ * @param accel_cal_y accel Y calibration
+ * @param accel_cal_z accel Z calibration
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_sensor_offsets_send(mavlink_channel_t chan, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z, float mag_declination, int32_t raw_press, int32_t raw_temp, float gyro_cal_x, float gyro_cal_y, float gyro_cal_z, float accel_cal_x, float accel_cal_y, float accel_cal_z)
+{
+	mavlink_message_t msg;
+	mavlink_msg_sensor_offsets_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, mag_ofs_x, mag_ofs_y, mag_ofs_z, mag_declination, raw_press, raw_temp, gyro_cal_x, gyro_cal_y, gyro_cal_z, accel_cal_x, accel_cal_y, accel_cal_z);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE SENSOR_OFFSETS UNPACKING
+
+/**
+ * @brief Get field mag_ofs_x from sensor_offsets message
+ *
+ * @return magnetometer X offset
+ */
+static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_x(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload)[0];
+	r.b[0] = (msg->payload)[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field mag_ofs_y from sensor_offsets message
+ *
+ * @return magnetometer Y offset
+ */
+static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_y(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field mag_ofs_z from sensor_offsets message
+ *
+ * @return magnetometer Z offset
+ */
+static inline int16_t mavlink_msg_sensor_offsets_get_mag_ofs_z(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field mag_declination from sensor_offsets message
+ *
+ * @return magnetic declination (radians)
+ */
+static inline float mavlink_msg_sensor_offsets_get_mag_declination(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0];
+	r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1];
+	r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[2];
+	r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field raw_press from sensor_offsets message
+ *
+ * @return raw pressure from barometer
+ */
+static inline int32_t mavlink_msg_sensor_offsets_get_raw_press(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float))[3];
+	return (int32_t)r.i;
+}
+
+/**
+ * @brief Get field raw_temp from sensor_offsets message
+ *
+ * @return raw temperature from barometer
+ */
+static inline int32_t mavlink_msg_sensor_offsets_get_raw_temp(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t))[0];
+	r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t))[1];
+	r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t))[2];
+	r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t))[3];
+	return (int32_t)r.i;
+}
+
+/**
+ * @brief Get field gyro_cal_x from sensor_offsets message
+ *
+ * @return gyro X calibration
+ */
+static inline float mavlink_msg_sensor_offsets_get_gyro_cal_x(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[0];
+	r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[1];
+	r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[2];
+	r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field gyro_cal_y from sensor_offsets message
+ *
+ * @return gyro Y calibration
+ */
+static inline float mavlink_msg_sensor_offsets_get_gyro_cal_y(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field gyro_cal_z from sensor_offsets message
+ *
+ * @return gyro Z calibration
+ */
+static inline float mavlink_msg_sensor_offsets_get_gyro_cal_z(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field accel_cal_x from sensor_offsets message
+ *
+ * @return accel X calibration
+ */
+static inline float mavlink_msg_sensor_offsets_get_accel_cal_x(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field accel_cal_y from sensor_offsets message
+ *
+ * @return accel Y calibration
+ */
+static inline float mavlink_msg_sensor_offsets_get_accel_cal_y(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field accel_cal_z from sensor_offsets message
+ *
+ * @return accel Z calibration
+ */
+static inline float mavlink_msg_sensor_offsets_get_accel_cal_z(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Decode a sensor_offsets message into a struct
+ *
+ * @param msg The message to decode
+ * @param sensor_offsets C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_sensor_offsets_decode(const mavlink_message_t* msg, mavlink_sensor_offsets_t* sensor_offsets)
+{
+	sensor_offsets->mag_ofs_x = mavlink_msg_sensor_offsets_get_mag_ofs_x(msg);
+	sensor_offsets->mag_ofs_y = mavlink_msg_sensor_offsets_get_mag_ofs_y(msg);
+	sensor_offsets->mag_ofs_z = mavlink_msg_sensor_offsets_get_mag_ofs_z(msg);
+	sensor_offsets->mag_declination = mavlink_msg_sensor_offsets_get_mag_declination(msg);
+	sensor_offsets->raw_press = mavlink_msg_sensor_offsets_get_raw_press(msg);
+	sensor_offsets->raw_temp = mavlink_msg_sensor_offsets_get_raw_temp(msg);
+	sensor_offsets->gyro_cal_x = mavlink_msg_sensor_offsets_get_gyro_cal_x(msg);
+	sensor_offsets->gyro_cal_y = mavlink_msg_sensor_offsets_get_gyro_cal_y(msg);
+	sensor_offsets->gyro_cal_z = mavlink_msg_sensor_offsets_get_gyro_cal_z(msg);
+	sensor_offsets->accel_cal_x = mavlink_msg_sensor_offsets_get_accel_cal_x(msg);
+	sensor_offsets->accel_cal_y = mavlink_msg_sensor_offsets_get_accel_cal_y(msg);
+	sensor_offsets->accel_cal_z = mavlink_msg_sensor_offsets_get_accel_cal_z(msg);
+}
diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h
new file mode 100644
index 0000000000000000000000000000000000000000..10dd1a58fd57d9158e18b3949b9d54303081f745
--- /dev/null
+++ b/thirdParty/mavlink/include/ardupilotmega/mavlink_msg_set_mag_offsets.h
@@ -0,0 +1,178 @@
+// MESSAGE SET_MAG_OFFSETS PACKING
+
+#define MAVLINK_MSG_ID_SET_MAG_OFFSETS 151
+
+typedef struct __mavlink_set_mag_offsets_t 
+{
+	uint8_t target_system; ///< System ID
+	uint8_t target_component; ///< Component ID
+	int16_t mag_ofs_x; ///< magnetometer X offset
+	int16_t mag_ofs_y; ///< magnetometer Y offset
+	int16_t mag_ofs_z; ///< magnetometer Z offset
+
+} mavlink_set_mag_offsets_t;
+
+
+
+/**
+ * @brief Pack a set_mag_offsets message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param mag_ofs_x magnetometer X offset
+ * @param mag_ofs_y magnetometer Y offset
+ * @param mag_ofs_z magnetometer Z offset
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_set_mag_offsets_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS;
+
+	i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
+	i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
+	i += put_int16_t_by_index(mag_ofs_x, i, msg->payload); // magnetometer X offset
+	i += put_int16_t_by_index(mag_ofs_y, i, msg->payload); // magnetometer Y offset
+	i += put_int16_t_by_index(mag_ofs_z, i, msg->payload); // magnetometer Z offset
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a set_mag_offsets message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param mag_ofs_x magnetometer X offset
+ * @param mag_ofs_y magnetometer Y offset
+ * @param mag_ofs_z magnetometer Z offset
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_set_mag_offsets_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_SET_MAG_OFFSETS;
+
+	i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
+	i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
+	i += put_int16_t_by_index(mag_ofs_x, i, msg->payload); // magnetometer X offset
+	i += put_int16_t_by_index(mag_ofs_y, i, msg->payload); // magnetometer Y offset
+	i += put_int16_t_by_index(mag_ofs_z, i, msg->payload); // magnetometer Z offset
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a set_mag_offsets struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param set_mag_offsets C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_set_mag_offsets_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_mag_offsets_t* set_mag_offsets)
+{
+	return mavlink_msg_set_mag_offsets_pack(system_id, component_id, msg, set_mag_offsets->target_system, set_mag_offsets->target_component, set_mag_offsets->mag_ofs_x, set_mag_offsets->mag_ofs_y, set_mag_offsets->mag_ofs_z);
+}
+
+/**
+ * @brief Send a set_mag_offsets message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param mag_ofs_x magnetometer X offset
+ * @param mag_ofs_y magnetometer Y offset
+ * @param mag_ofs_z magnetometer Z offset
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_set_mag_offsets_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t mag_ofs_x, int16_t mag_ofs_y, int16_t mag_ofs_z)
+{
+	mavlink_message_t msg;
+	mavlink_msg_set_mag_offsets_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE SET_MAG_OFFSETS UNPACKING
+
+/**
+ * @brief Get field target_system from set_mag_offsets message
+ *
+ * @return System ID
+ */
+static inline uint8_t mavlink_msg_set_mag_offsets_get_target_system(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload)[0];
+}
+
+/**
+ * @brief Get field target_component from set_mag_offsets message
+ *
+ * @return Component ID
+ */
+static inline uint8_t mavlink_msg_set_mag_offsets_get_target_component(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload+sizeof(uint8_t))[0];
+}
+
+/**
+ * @brief Get field mag_ofs_x from set_mag_offsets message
+ *
+ * @return magnetometer X offset
+ */
+static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_x(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field mag_ofs_y from set_mag_offsets message
+ *
+ * @return magnetometer Y offset
+ */
+static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_y(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field mag_ofs_z from set_mag_offsets message
+ *
+ * @return magnetometer Z offset
+ */
+static inline int16_t mavlink_msg_set_mag_offsets_get_mag_ofs_z(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Decode a set_mag_offsets message into a struct
+ *
+ * @param msg The message to decode
+ * @param set_mag_offsets C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_set_mag_offsets_decode(const mavlink_message_t* msg, mavlink_set_mag_offsets_t* set_mag_offsets)
+{
+	set_mag_offsets->target_system = mavlink_msg_set_mag_offsets_get_target_system(msg);
+	set_mag_offsets->target_component = mavlink_msg_set_mag_offsets_get_target_component(msg);
+	set_mag_offsets->mag_ofs_x = mavlink_msg_set_mag_offsets_get_mag_ofs_x(msg);
+	set_mag_offsets->mag_ofs_y = mavlink_msg_set_mag_offsets_get_mag_ofs_y(msg);
+	set_mag_offsets->mag_ofs_z = mavlink_msg_set_mag_offsets_get_mag_ofs_z(msg);
+}
diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h
index bf99ef000314ed026055dca19e22cc106f0e59fe..8e0348723e64b6ba0682af65d5323453e194b243 100644
--- a/thirdParty/mavlink/include/common/common.h
+++ b/thirdParty/mavlink/include/common/common.h
@@ -1,7 +1,7 @@
 /** @file
  *	@brief MAVLink comm protocol.
  *	@see http://qgroundcontrol.org/mavlink/
- *	 Generated on Wednesday, July 27 2011, 14:17 UTC
+ *	 Generated on Monday, August 22 2011, 15:48 UTC
  */
 #ifndef COMMON_H
 #define COMMON_H
@@ -55,7 +55,8 @@ enum MAV_CMD
 	MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. | Relay number | Cycle count | Cycle time (seconds, decimal) | Empty | Empty | Empty | Empty | */
 	MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. | Servo number | PWM (microseconds, 1000 to 2000 typical) | Empty | Empty | Empty | Empty | Empty | */
 	MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. | Servo number | PWM (microseconds, 1000 to 2000 typical) | Cycle count | Cycle time (seconds) | Empty | Empty | Empty | */
-	MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. | Camera ID (-1 for all) | Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw | Transmission mode: 0: video stream, >0: single images every n seconds (decimal) | Recording: 0: disabled, 1: enabled compressed, 2: enabled raw | Empty | Empty | Empty | */
+	MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera capturing. | Camera ID (-1 for all) | Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw | Transmission mode: 0: video stream, >0: single images every n seconds (decimal) | Recording: 0: disabled, 1: enabled compressed, 2: enabled raw | Empty | Empty | Empty | */
+	MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various devices such as cameras. | Region of interest mode. (see MAV_ROI enum) | Waypoint index/ target ID. (see MAV_ROI enum) | ROI index (allows a vehicle to manage multiple cameras etc.) | Empty | x the location of the fixed ROI (see MAV_FRAME) | y | z | */
 	MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration | Empty | Empty | Empty | Empty | Empty | Empty | Empty | */
 	MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. | Gyro calibration: 0: no, 1: yes | Magnetometer calibration: 0: no, 1: yes | Ground pressure: 0: no, 1: yes | Radio calibration: 0: no, 1: yes | Empty | Empty | Empty | */
 	MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. | Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM | Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM | Reserved | Reserved | Empty | Empty | Empty | */
@@ -77,13 +78,14 @@ enum MAV_DATA_STREAM
 	MAV_DATA_STREAM_ENUM_END
 };
 
-/** @brief   The ROI (region of interest) for the vehicle. This can be       be used by the vehicle for camera/vehicle attitude alignment (see 	  MAV_CMD_NAV_ROI).       */
+/** @brief   The ROI (region of interest) for the vehicle. This can be                 be used by the vehicle for camera/vehicle attitude alignment (see                 MAV_CMD_NAV_ROI).              */
 enum MAV_ROI
 {
-	MAV_ROI_WPNEXT=0, /* Point toward next waypoint. | */
-	MAV_ROI_WPINDEX=1, /* Point toward given waypoint. | */
-	MAV_ROI_LOCATION=2, /* Point toward fixed location. | */
-	MAV_ROI_TARGET=3, /* Point toward of given id. | */
+	MAV_ROI_NONE=0, /* No region of interest. | */
+	MAV_ROI_WPNEXT=1, /* Point toward next waypoint. | */
+	MAV_ROI_WPINDEX=2, /* Point toward given waypoint. | */
+	MAV_ROI_LOCATION=3, /* Point toward fixed location. | */
+	MAV_ROI_TARGET=4, /* Point toward of given id. | */
 	MAV_ROI_ENUM_END
 };
 
@@ -136,18 +138,25 @@ enum MAV_ROI
 #include "./mavlink_msg_control_status.h"
 #include "./mavlink_msg_safety_set_allowed_area.h"
 #include "./mavlink_msg_safety_allowed_area.h"
-#include "./mavlink_msg_attitude_controller_output.h"
-#include "./mavlink_msg_position_controller_output.h"
+#include "./mavlink_msg_set_roll_pitch_yaw_thrust.h"
+#include "./mavlink_msg_set_roll_pitch_yaw_speed_thrust.h"
+#include "./mavlink_msg_roll_pitch_yaw_thrust_setpoint.h"
+#include "./mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h"
 #include "./mavlink_msg_nav_controller_output.h"
 #include "./mavlink_msg_position_target.h"
 #include "./mavlink_msg_state_correction.h"
 #include "./mavlink_msg_set_altitude.h"
 #include "./mavlink_msg_request_data_stream.h"
+#include "./mavlink_msg_hil_state.h"
+#include "./mavlink_msg_hil_controls.h"
 #include "./mavlink_msg_manual_control.h"
+#include "./mavlink_msg_rc_channels_override.h"
 #include "./mavlink_msg_global_position_int.h"
 #include "./mavlink_msg_vfr_hud.h"
 #include "./mavlink_msg_command.h"
 #include "./mavlink_msg_command_ack.h"
+#include "./mavlink_msg_optical_flow.h"
+#include "./mavlink_msg_object_detection_event.h"
 #include "./mavlink_msg_debug_vect.h"
 #include "./mavlink_msg_named_value_float.h"
 #include "./mavlink_msg_named_value_int.h"
@@ -158,7 +167,7 @@ enum MAV_ROI
 // MESSAGE LENGTHS
 
 #undef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
+#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
 
 #ifdef __cplusplus
 }
diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h
index 404b970bffead60f0b273e47333bd8f438f488db..86a5baf89ca82ee6d87b81337a492c583787010f 100644
--- a/thirdParty/mavlink/include/common/mavlink.h
+++ b/thirdParty/mavlink/include/common/mavlink.h
@@ -1,7 +1,7 @@
 /** @file
  *	@brief MAVLink comm protocol.
  *	@see http://pixhawk.ethz.ch/software/mavlink
- *	 Generated on Wednesday, July 27 2011, 14:17 UTC
+ *	 Generated on Monday, August 22 2011, 15:48 UTC
  */
 #ifndef MAVLINK_H
 #define MAVLINK_H
diff --git a/thirdParty/mavlink/include/common/mavlink_msg_full_state.h b/thirdParty/mavlink/include/common/mavlink_msg_full_state.h
new file mode 100644
index 0000000000000000000000000000000000000000..ed177e22e2fcf3ab2494211338d711e4b3f2b51c
--- /dev/null
+++ b/thirdParty/mavlink/include/common/mavlink_msg_full_state.h
@@ -0,0 +1,428 @@
+// MESSAGE FULL_STATE PACKING
+
+#define MAVLINK_MSG_ID_FULL_STATE 67
+
+typedef struct __mavlink_full_state_t 
+{
+	uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+	float roll; ///< Roll angle (rad)
+	float pitch; ///< Pitch angle (rad)
+	float yaw; ///< Yaw angle (rad)
+	float rollspeed; ///< Roll angular speed (rad/s)
+	float pitchspeed; ///< Pitch angular speed (rad/s)
+	float yawspeed; ///< Yaw angular speed (rad/s)
+	int32_t lat; ///< Latitude, expressed as * 1E7
+	int32_t lon; ///< Longitude, expressed as * 1E7
+	int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters)
+	int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100
+	int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100
+	int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100
+	int16_t xacc; ///< X acceleration (mg)
+	int16_t yacc; ///< Y acceleration (mg)
+	int16_t zacc; ///< Z acceleration (mg)
+
+} mavlink_full_state_t;
+
+
+
+/**
+ * @brief Pack a full_state message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+ * @param roll Roll angle (rad)
+ * @param pitch Pitch angle (rad)
+ * @param yaw Yaw angle (rad)
+ * @param rollspeed Roll angular speed (rad/s)
+ * @param pitchspeed Pitch angular speed (rad/s)
+ * @param yawspeed Yaw angular speed (rad/s)
+ * @param lat Latitude, expressed as * 1E7
+ * @param lon Longitude, expressed as * 1E7
+ * @param alt Altitude in meters, expressed as * 1000 (millimeters)
+ * @param vx Ground X Speed (Latitude), expressed as m/s * 100
+ * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
+ * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
+ * @param xacc X acceleration (mg)
+ * @param yacc Y acceleration (mg)
+ * @param zacc Z acceleration (mg)
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_full_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_FULL_STATE;
+
+	i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+	i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad)
+	i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad)
+	i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad)
+	i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s)
+	i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s)
+	i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s)
+	i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7
+	i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7
+	i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters)
+	i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100
+	i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100
+	i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100
+	i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg)
+	i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg)
+	i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg)
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a full_state message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+ * @param roll Roll angle (rad)
+ * @param pitch Pitch angle (rad)
+ * @param yaw Yaw angle (rad)
+ * @param rollspeed Roll angular speed (rad/s)
+ * @param pitchspeed Pitch angular speed (rad/s)
+ * @param yawspeed Yaw angular speed (rad/s)
+ * @param lat Latitude, expressed as * 1E7
+ * @param lon Longitude, expressed as * 1E7
+ * @param alt Altitude in meters, expressed as * 1000 (millimeters)
+ * @param vx Ground X Speed (Latitude), expressed as m/s * 100
+ * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
+ * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
+ * @param xacc X acceleration (mg)
+ * @param yacc Y acceleration (mg)
+ * @param zacc Z acceleration (mg)
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_full_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_FULL_STATE;
+
+	i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+	i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad)
+	i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad)
+	i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad)
+	i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s)
+	i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s)
+	i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s)
+	i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7
+	i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7
+	i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters)
+	i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100
+	i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100
+	i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100
+	i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg)
+	i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg)
+	i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg)
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a full_state struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param full_state C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_full_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_full_state_t* full_state)
+{
+	return mavlink_msg_full_state_pack(system_id, component_id, msg, full_state->usec, full_state->roll, full_state->pitch, full_state->yaw, full_state->rollspeed, full_state->pitchspeed, full_state->yawspeed, full_state->lat, full_state->lon, full_state->alt, full_state->vx, full_state->vy, full_state->vz, full_state->xacc, full_state->yacc, full_state->zacc);
+}
+
+/**
+ * @brief Send a full_state message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+ * @param roll Roll angle (rad)
+ * @param pitch Pitch angle (rad)
+ * @param yaw Yaw angle (rad)
+ * @param rollspeed Roll angular speed (rad/s)
+ * @param pitchspeed Pitch angular speed (rad/s)
+ * @param yawspeed Yaw angular speed (rad/s)
+ * @param lat Latitude, expressed as * 1E7
+ * @param lon Longitude, expressed as * 1E7
+ * @param alt Altitude in meters, expressed as * 1000 (millimeters)
+ * @param vx Ground X Speed (Latitude), expressed as m/s * 100
+ * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
+ * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
+ * @param xacc X acceleration (mg)
+ * @param yacc Y acceleration (mg)
+ * @param zacc Z acceleration (mg)
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_full_state_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
+{
+	mavlink_message_t msg;
+	mavlink_msg_full_state_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE FULL_STATE UNPACKING
+
+/**
+ * @brief Get field usec from full_state message
+ *
+ * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+ */
+static inline uint64_t mavlink_msg_full_state_get_usec(const mavlink_message_t* msg)
+{
+	generic_64bit r;
+	r.b[7] = (msg->payload)[0];
+	r.b[6] = (msg->payload)[1];
+	r.b[5] = (msg->payload)[2];
+	r.b[4] = (msg->payload)[3];
+	r.b[3] = (msg->payload)[4];
+	r.b[2] = (msg->payload)[5];
+	r.b[1] = (msg->payload)[6];
+	r.b[0] = (msg->payload)[7];
+	return (uint64_t)r.ll;
+}
+
+/**
+ * @brief Get field roll from full_state message
+ *
+ * @return Roll angle (rad)
+ */
+static inline float mavlink_msg_full_state_get_roll(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field pitch from full_state message
+ *
+ * @return Pitch angle (rad)
+ */
+static inline float mavlink_msg_full_state_get_pitch(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field yaw from full_state message
+ *
+ * @return Yaw angle (rad)
+ */
+static inline float mavlink_msg_full_state_get_yaw(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field rollspeed from full_state message
+ *
+ * @return Roll angular speed (rad/s)
+ */
+static inline float mavlink_msg_full_state_get_rollspeed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field pitchspeed from full_state message
+ *
+ * @return Pitch angular speed (rad/s)
+ */
+static inline float mavlink_msg_full_state_get_pitchspeed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field yawspeed from full_state message
+ *
+ * @return Yaw angular speed (rad/s)
+ */
+static inline float mavlink_msg_full_state_get_yawspeed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field lat from full_state message
+ *
+ * @return Latitude, expressed as * 1E7
+ */
+static inline int32_t mavlink_msg_full_state_get_lat(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (int32_t)r.i;
+}
+
+/**
+ * @brief Get field lon from full_state message
+ *
+ * @return Longitude, expressed as * 1E7
+ */
+static inline int32_t mavlink_msg_full_state_get_lon(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[3];
+	return (int32_t)r.i;
+}
+
+/**
+ * @brief Get field alt from full_state message
+ *
+ * @return Altitude in meters, expressed as * 1000 (millimeters)
+ */
+static inline int32_t mavlink_msg_full_state_get_alt(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[3];
+	return (int32_t)r.i;
+}
+
+/**
+ * @brief Get field vx from full_state message
+ *
+ * @return Ground X Speed (Latitude), expressed as m/s * 100
+ */
+static inline int16_t mavlink_msg_full_state_get_vx(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field vy from full_state message
+ *
+ * @return Ground Y Speed (Longitude), expressed as m/s * 100
+ */
+static inline int16_t mavlink_msg_full_state_get_vy(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field vz from full_state message
+ *
+ * @return Ground Z Speed (Altitude), expressed as m/s * 100
+ */
+static inline int16_t mavlink_msg_full_state_get_vz(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field xacc from full_state message
+ *
+ * @return X acceleration (mg)
+ */
+static inline int16_t mavlink_msg_full_state_get_xacc(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field yacc from full_state message
+ *
+ * @return Y acceleration (mg)
+ */
+static inline int16_t mavlink_msg_full_state_get_yacc(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field zacc from full_state message
+ *
+ * @return Z acceleration (mg)
+ */
+static inline int16_t mavlink_msg_full_state_get_zacc(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Decode a full_state message into a struct
+ *
+ * @param msg The message to decode
+ * @param full_state C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_full_state_decode(const mavlink_message_t* msg, mavlink_full_state_t* full_state)
+{
+	full_state->usec = mavlink_msg_full_state_get_usec(msg);
+	full_state->roll = mavlink_msg_full_state_get_roll(msg);
+	full_state->pitch = mavlink_msg_full_state_get_pitch(msg);
+	full_state->yaw = mavlink_msg_full_state_get_yaw(msg);
+	full_state->rollspeed = mavlink_msg_full_state_get_rollspeed(msg);
+	full_state->pitchspeed = mavlink_msg_full_state_get_pitchspeed(msg);
+	full_state->yawspeed = mavlink_msg_full_state_get_yawspeed(msg);
+	full_state->lat = mavlink_msg_full_state_get_lat(msg);
+	full_state->lon = mavlink_msg_full_state_get_lon(msg);
+	full_state->alt = mavlink_msg_full_state_get_alt(msg);
+	full_state->vx = mavlink_msg_full_state_get_vx(msg);
+	full_state->vy = mavlink_msg_full_state_get_vy(msg);
+	full_state->vz = mavlink_msg_full_state_get_vz(msg);
+	full_state->xacc = mavlink_msg_full_state_get_xacc(msg);
+	full_state->yacc = mavlink_msg_full_state_get_yacc(msg);
+	full_state->zacc = mavlink_msg_full_state_get_zacc(msg);
+}
diff --git a/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h b/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h
new file mode 100644
index 0000000000000000000000000000000000000000..80f33ebd06a9497d2d3f8678d8a104960d280926
--- /dev/null
+++ b/thirdParty/mavlink/include/common/mavlink_msg_hil_controls.h
@@ -0,0 +1,232 @@
+// MESSAGE HIL_CONTROLS PACKING
+
+#define MAVLINK_MSG_ID_HIL_CONTROLS 68
+
+typedef struct __mavlink_hil_controls_t 
+{
+	uint64_t time_us; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+	float roll_ailerons; ///< Control output -3 .. 1
+	float pitch_elevator; ///< Control output -1 .. 1
+	float yaw_rudder; ///< Control output -1 .. 1
+	float throttle; ///< Throttle 0 .. 1
+	uint8_t mode; ///< System mode (MAV_MODE)
+	uint8_t nav_mode; ///< Navigation mode (MAV_NAV_MODE)
+
+} mavlink_hil_controls_t;
+
+
+
+/**
+ * @brief Pack a hil_controls message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+ * @param roll_ailerons Control output -3 .. 1
+ * @param pitch_elevator Control output -1 .. 1
+ * @param yaw_rudder Control output -1 .. 1
+ * @param throttle Throttle 0 .. 1
+ * @param mode System mode (MAV_MODE)
+ * @param nav_mode Navigation mode (MAV_NAV_MODE)
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_hil_controls_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, uint8_t mode, uint8_t nav_mode)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS;
+
+	i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+	i += put_float_by_index(roll_ailerons, i, msg->payload); // Control output -3 .. 1
+	i += put_float_by_index(pitch_elevator, i, msg->payload); // Control output -1 .. 1
+	i += put_float_by_index(yaw_rudder, i, msg->payload); // Control output -1 .. 1
+	i += put_float_by_index(throttle, i, msg->payload); // Throttle 0 .. 1
+	i += put_uint8_t_by_index(mode, i, msg->payload); // System mode (MAV_MODE)
+	i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode (MAV_NAV_MODE)
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a hil_controls message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+ * @param roll_ailerons Control output -3 .. 1
+ * @param pitch_elevator Control output -1 .. 1
+ * @param yaw_rudder Control output -1 .. 1
+ * @param throttle Throttle 0 .. 1
+ * @param mode System mode (MAV_MODE)
+ * @param nav_mode Navigation mode (MAV_NAV_MODE)
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_hil_controls_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, uint8_t mode, uint8_t nav_mode)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_HIL_CONTROLS;
+
+	i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+	i += put_float_by_index(roll_ailerons, i, msg->payload); // Control output -3 .. 1
+	i += put_float_by_index(pitch_elevator, i, msg->payload); // Control output -1 .. 1
+	i += put_float_by_index(yaw_rudder, i, msg->payload); // Control output -1 .. 1
+	i += put_float_by_index(throttle, i, msg->payload); // Throttle 0 .. 1
+	i += put_uint8_t_by_index(mode, i, msg->payload); // System mode (MAV_MODE)
+	i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode (MAV_NAV_MODE)
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a hil_controls struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param hil_controls C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_hil_controls_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_controls_t* hil_controls)
+{
+	return mavlink_msg_hil_controls_pack(system_id, component_id, msg, hil_controls->time_us, hil_controls->roll_ailerons, hil_controls->pitch_elevator, hil_controls->yaw_rudder, hil_controls->throttle, hil_controls->mode, hil_controls->nav_mode);
+}
+
+/**
+ * @brief Send a hil_controls message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param time_us Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+ * @param roll_ailerons Control output -3 .. 1
+ * @param pitch_elevator Control output -1 .. 1
+ * @param yaw_rudder Control output -1 .. 1
+ * @param throttle Throttle 0 .. 1
+ * @param mode System mode (MAV_MODE)
+ * @param nav_mode Navigation mode (MAV_NAV_MODE)
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_hil_controls_send(mavlink_channel_t chan, uint64_t time_us, float roll_ailerons, float pitch_elevator, float yaw_rudder, float throttle, uint8_t mode, uint8_t nav_mode)
+{
+	mavlink_message_t msg;
+	mavlink_msg_hil_controls_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE HIL_CONTROLS UNPACKING
+
+/**
+ * @brief Get field time_us from hil_controls message
+ *
+ * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+ */
+static inline uint64_t mavlink_msg_hil_controls_get_time_us(const mavlink_message_t* msg)
+{
+	generic_64bit r;
+	r.b[7] = (msg->payload)[0];
+	r.b[6] = (msg->payload)[1];
+	r.b[5] = (msg->payload)[2];
+	r.b[4] = (msg->payload)[3];
+	r.b[3] = (msg->payload)[4];
+	r.b[2] = (msg->payload)[5];
+	r.b[1] = (msg->payload)[6];
+	r.b[0] = (msg->payload)[7];
+	return (uint64_t)r.ll;
+}
+
+/**
+ * @brief Get field roll_ailerons from hil_controls message
+ *
+ * @return Control output -3 .. 1
+ */
+static inline float mavlink_msg_hil_controls_get_roll_ailerons(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field pitch_elevator from hil_controls message
+ *
+ * @return Control output -1 .. 1
+ */
+static inline float mavlink_msg_hil_controls_get_pitch_elevator(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field yaw_rudder from hil_controls message
+ *
+ * @return Control output -1 .. 1
+ */
+static inline float mavlink_msg_hil_controls_get_yaw_rudder(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field throttle from hil_controls message
+ *
+ * @return Throttle 0 .. 1
+ */
+static inline float mavlink_msg_hil_controls_get_throttle(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field mode from hil_controls message
+ *
+ * @return System mode (MAV_MODE)
+ */
+static inline uint8_t mavlink_msg_hil_controls_get_mode(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+}
+
+/**
+ * @brief Get field nav_mode from hil_controls message
+ *
+ * @return Navigation mode (MAV_NAV_MODE)
+ */
+static inline uint8_t mavlink_msg_hil_controls_get_nav_mode(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(uint8_t))[0];
+}
+
+/**
+ * @brief Decode a hil_controls message into a struct
+ *
+ * @param msg The message to decode
+ * @param hil_controls C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_hil_controls_decode(const mavlink_message_t* msg, mavlink_hil_controls_t* hil_controls)
+{
+	hil_controls->time_us = mavlink_msg_hil_controls_get_time_us(msg);
+	hil_controls->roll_ailerons = mavlink_msg_hil_controls_get_roll_ailerons(msg);
+	hil_controls->pitch_elevator = mavlink_msg_hil_controls_get_pitch_elevator(msg);
+	hil_controls->yaw_rudder = mavlink_msg_hil_controls_get_yaw_rudder(msg);
+	hil_controls->throttle = mavlink_msg_hil_controls_get_throttle(msg);
+	hil_controls->mode = mavlink_msg_hil_controls_get_mode(msg);
+	hil_controls->nav_mode = mavlink_msg_hil_controls_get_nav_mode(msg);
+}
diff --git a/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h b/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h
new file mode 100644
index 0000000000000000000000000000000000000000..371cd4ae46255970abd252901ef905bd96ddf9af
--- /dev/null
+++ b/thirdParty/mavlink/include/common/mavlink_msg_hil_state.h
@@ -0,0 +1,428 @@
+// MESSAGE HIL_STATE PACKING
+
+#define MAVLINK_MSG_ID_HIL_STATE 67
+
+typedef struct __mavlink_hil_state_t 
+{
+	uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+	float roll; ///< Roll angle (rad)
+	float pitch; ///< Pitch angle (rad)
+	float yaw; ///< Yaw angle (rad)
+	float rollspeed; ///< Roll angular speed (rad/s)
+	float pitchspeed; ///< Pitch angular speed (rad/s)
+	float yawspeed; ///< Yaw angular speed (rad/s)
+	int32_t lat; ///< Latitude, expressed as * 1E7
+	int32_t lon; ///< Longitude, expressed as * 1E7
+	int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters)
+	int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100
+	int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100
+	int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100
+	int16_t xacc; ///< X acceleration (mg)
+	int16_t yacc; ///< Y acceleration (mg)
+	int16_t zacc; ///< Z acceleration (mg)
+
+} mavlink_hil_state_t;
+
+
+
+/**
+ * @brief Pack a hil_state message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+ * @param roll Roll angle (rad)
+ * @param pitch Pitch angle (rad)
+ * @param yaw Yaw angle (rad)
+ * @param rollspeed Roll angular speed (rad/s)
+ * @param pitchspeed Pitch angular speed (rad/s)
+ * @param yawspeed Yaw angular speed (rad/s)
+ * @param lat Latitude, expressed as * 1E7
+ * @param lon Longitude, expressed as * 1E7
+ * @param alt Altitude in meters, expressed as * 1000 (millimeters)
+ * @param vx Ground X Speed (Latitude), expressed as m/s * 100
+ * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
+ * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
+ * @param xacc X acceleration (mg)
+ * @param yacc Y acceleration (mg)
+ * @param zacc Z acceleration (mg)
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_hil_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_HIL_STATE;
+
+	i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+	i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad)
+	i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad)
+	i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad)
+	i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s)
+	i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s)
+	i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s)
+	i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7
+	i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7
+	i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters)
+	i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100
+	i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100
+	i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100
+	i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg)
+	i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg)
+	i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg)
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a hil_state message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+ * @param roll Roll angle (rad)
+ * @param pitch Pitch angle (rad)
+ * @param yaw Yaw angle (rad)
+ * @param rollspeed Roll angular speed (rad/s)
+ * @param pitchspeed Pitch angular speed (rad/s)
+ * @param yawspeed Yaw angular speed (rad/s)
+ * @param lat Latitude, expressed as * 1E7
+ * @param lon Longitude, expressed as * 1E7
+ * @param alt Altitude in meters, expressed as * 1000 (millimeters)
+ * @param vx Ground X Speed (Latitude), expressed as m/s * 100
+ * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
+ * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
+ * @param xacc X acceleration (mg)
+ * @param yacc Y acceleration (mg)
+ * @param zacc Z acceleration (mg)
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_hil_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_HIL_STATE;
+
+	i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+	i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad)
+	i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad)
+	i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad)
+	i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s)
+	i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s)
+	i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s)
+	i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7
+	i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7
+	i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters)
+	i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100
+	i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100
+	i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100
+	i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg)
+	i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg)
+	i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg)
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a hil_state struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param hil_state C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_hil_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_state_t* hil_state)
+{
+	return mavlink_msg_hil_state_pack(system_id, component_id, msg, hil_state->usec, hil_state->roll, hil_state->pitch, hil_state->yaw, hil_state->rollspeed, hil_state->pitchspeed, hil_state->yawspeed, hil_state->lat, hil_state->lon, hil_state->alt, hil_state->vx, hil_state->vy, hil_state->vz, hil_state->xacc, hil_state->yacc, hil_state->zacc);
+}
+
+/**
+ * @brief Send a hil_state message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+ * @param roll Roll angle (rad)
+ * @param pitch Pitch angle (rad)
+ * @param yaw Yaw angle (rad)
+ * @param rollspeed Roll angular speed (rad/s)
+ * @param pitchspeed Pitch angular speed (rad/s)
+ * @param yawspeed Yaw angular speed (rad/s)
+ * @param lat Latitude, expressed as * 1E7
+ * @param lon Longitude, expressed as * 1E7
+ * @param alt Altitude in meters, expressed as * 1000 (millimeters)
+ * @param vx Ground X Speed (Latitude), expressed as m/s * 100
+ * @param vy Ground Y Speed (Longitude), expressed as m/s * 100
+ * @param vz Ground Z Speed (Altitude), expressed as m/s * 100
+ * @param xacc X acceleration (mg)
+ * @param yacc Y acceleration (mg)
+ * @param zacc Z acceleration (mg)
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_hil_state_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
+{
+	mavlink_message_t msg;
+	mavlink_msg_hil_state_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE HIL_STATE UNPACKING
+
+/**
+ * @brief Get field usec from hil_state message
+ *
+ * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
+ */
+static inline uint64_t mavlink_msg_hil_state_get_usec(const mavlink_message_t* msg)
+{
+	generic_64bit r;
+	r.b[7] = (msg->payload)[0];
+	r.b[6] = (msg->payload)[1];
+	r.b[5] = (msg->payload)[2];
+	r.b[4] = (msg->payload)[3];
+	r.b[3] = (msg->payload)[4];
+	r.b[2] = (msg->payload)[5];
+	r.b[1] = (msg->payload)[6];
+	r.b[0] = (msg->payload)[7];
+	return (uint64_t)r.ll;
+}
+
+/**
+ * @brief Get field roll from hil_state message
+ *
+ * @return Roll angle (rad)
+ */
+static inline float mavlink_msg_hil_state_get_roll(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field pitch from hil_state message
+ *
+ * @return Pitch angle (rad)
+ */
+static inline float mavlink_msg_hil_state_get_pitch(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field yaw from hil_state message
+ *
+ * @return Yaw angle (rad)
+ */
+static inline float mavlink_msg_hil_state_get_yaw(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field rollspeed from hil_state message
+ *
+ * @return Roll angular speed (rad/s)
+ */
+static inline float mavlink_msg_hil_state_get_rollspeed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field pitchspeed from hil_state message
+ *
+ * @return Pitch angular speed (rad/s)
+ */
+static inline float mavlink_msg_hil_state_get_pitchspeed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field yawspeed from hil_state message
+ *
+ * @return Yaw angular speed (rad/s)
+ */
+static inline float mavlink_msg_hil_state_get_yawspeed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field lat from hil_state message
+ *
+ * @return Latitude, expressed as * 1E7
+ */
+static inline int32_t mavlink_msg_hil_state_get_lat(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (int32_t)r.i;
+}
+
+/**
+ * @brief Get field lon from hil_state message
+ *
+ * @return Longitude, expressed as * 1E7
+ */
+static inline int32_t mavlink_msg_hil_state_get_lon(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[3];
+	return (int32_t)r.i;
+}
+
+/**
+ * @brief Get field alt from hil_state message
+ *
+ * @return Altitude in meters, expressed as * 1000 (millimeters)
+ */
+static inline int32_t mavlink_msg_hil_state_get_alt(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[3];
+	return (int32_t)r.i;
+}
+
+/**
+ * @brief Get field vx from hil_state message
+ *
+ * @return Ground X Speed (Latitude), expressed as m/s * 100
+ */
+static inline int16_t mavlink_msg_hil_state_get_vx(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field vy from hil_state message
+ *
+ * @return Ground Y Speed (Longitude), expressed as m/s * 100
+ */
+static inline int16_t mavlink_msg_hil_state_get_vy(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field vz from hil_state message
+ *
+ * @return Ground Z Speed (Altitude), expressed as m/s * 100
+ */
+static inline int16_t mavlink_msg_hil_state_get_vz(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field xacc from hil_state message
+ *
+ * @return X acceleration (mg)
+ */
+static inline int16_t mavlink_msg_hil_state_get_xacc(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field yacc from hil_state message
+ *
+ * @return Y acceleration (mg)
+ */
+static inline int16_t mavlink_msg_hil_state_get_yacc(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field zacc from hil_state message
+ *
+ * @return Z acceleration (mg)
+ */
+static inline int16_t mavlink_msg_hil_state_get_zacc(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Decode a hil_state message into a struct
+ *
+ * @param msg The message to decode
+ * @param hil_state C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_hil_state_decode(const mavlink_message_t* msg, mavlink_hil_state_t* hil_state)
+{
+	hil_state->usec = mavlink_msg_hil_state_get_usec(msg);
+	hil_state->roll = mavlink_msg_hil_state_get_roll(msg);
+	hil_state->pitch = mavlink_msg_hil_state_get_pitch(msg);
+	hil_state->yaw = mavlink_msg_hil_state_get_yaw(msg);
+	hil_state->rollspeed = mavlink_msg_hil_state_get_rollspeed(msg);
+	hil_state->pitchspeed = mavlink_msg_hil_state_get_pitchspeed(msg);
+	hil_state->yawspeed = mavlink_msg_hil_state_get_yawspeed(msg);
+	hil_state->lat = mavlink_msg_hil_state_get_lat(msg);
+	hil_state->lon = mavlink_msg_hil_state_get_lon(msg);
+	hil_state->alt = mavlink_msg_hil_state_get_alt(msg);
+	hil_state->vx = mavlink_msg_hil_state_get_vx(msg);
+	hil_state->vy = mavlink_msg_hil_state_get_vy(msg);
+	hil_state->vz = mavlink_msg_hil_state_get_vz(msg);
+	hil_state->xacc = mavlink_msg_hil_state_get_xacc(msg);
+	hil_state->yacc = mavlink_msg_hil_state_get_yacc(msg);
+	hil_state->zacc = mavlink_msg_hil_state_get_zacc(msg);
+}
diff --git a/thirdParty/mavlink/include/common/mavlink_msg_object_detection_event.h b/thirdParty/mavlink/include/common/mavlink_msg_object_detection_event.h
new file mode 100644
index 0000000000000000000000000000000000000000..bd45e811ef22de3f4a3e4dbd1de13bcf3d0bf142
--- /dev/null
+++ b/thirdParty/mavlink/include/common/mavlink_msg_object_detection_event.h
@@ -0,0 +1,224 @@
+// MESSAGE OBJECT_DETECTION_EVENT PACKING
+
+#define MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT 140
+
+typedef struct __mavlink_object_detection_event_t 
+{
+	uint32_t time; ///< Timestamp in milliseconds since system boot
+	uint16_t object_id; ///< Object ID
+	uint8_t type; ///< Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal
+	char name[20]; ///< Name of the object as defined by the detector
+	uint8_t quality; ///< Detection quality / confidence. 0: bad, 255: maximum confidence
+	float bearing; ///< Angle of the object with respect to the body frame in NED coordinates in radians. 0: front
+	float distance; ///< Ground distance in meters
+
+} mavlink_object_detection_event_t;
+
+#define MAVLINK_MSG_OBJECT_DETECTION_EVENT_FIELD_NAME_LEN 20
+
+
+/**
+ * @brief Pack a object_detection_event message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param time Timestamp in milliseconds since system boot
+ * @param object_id Object ID
+ * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal
+ * @param name Name of the object as defined by the detector
+ * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence
+ * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front
+ * @param distance Ground distance in meters
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_object_detection_event_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time, uint16_t object_id, uint8_t type, const char* name, uint8_t quality, float bearing, float distance)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT;
+
+	i += put_uint32_t_by_index(time, i, msg->payload); // Timestamp in milliseconds since system boot
+	i += put_uint16_t_by_index(object_id, i, msg->payload); // Object ID
+	i += put_uint8_t_by_index(type, i, msg->payload); // Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal
+	i += put_array_by_index((const int8_t*)name, sizeof(char)*20, i, msg->payload); // Name of the object as defined by the detector
+	i += put_uint8_t_by_index(quality, i, msg->payload); // Detection quality / confidence. 0: bad, 255: maximum confidence
+	i += put_float_by_index(bearing, i, msg->payload); // Angle of the object with respect to the body frame in NED coordinates in radians. 0: front
+	i += put_float_by_index(distance, i, msg->payload); // Ground distance in meters
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a object_detection_event message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param time Timestamp in milliseconds since system boot
+ * @param object_id Object ID
+ * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal
+ * @param name Name of the object as defined by the detector
+ * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence
+ * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front
+ * @param distance Ground distance in meters
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_object_detection_event_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t time, uint16_t object_id, uint8_t type, const char* name, uint8_t quality, float bearing, float distance)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT;
+
+	i += put_uint32_t_by_index(time, i, msg->payload); // Timestamp in milliseconds since system boot
+	i += put_uint16_t_by_index(object_id, i, msg->payload); // Object ID
+	i += put_uint8_t_by_index(type, i, msg->payload); // Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal
+	i += put_array_by_index((const int8_t*)name, sizeof(char)*20, i, msg->payload); // Name of the object as defined by the detector
+	i += put_uint8_t_by_index(quality, i, msg->payload); // Detection quality / confidence. 0: bad, 255: maximum confidence
+	i += put_float_by_index(bearing, i, msg->payload); // Angle of the object with respect to the body frame in NED coordinates in radians. 0: front
+	i += put_float_by_index(distance, i, msg->payload); // Ground distance in meters
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a object_detection_event struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param object_detection_event C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_object_detection_event_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_object_detection_event_t* object_detection_event)
+{
+	return mavlink_msg_object_detection_event_pack(system_id, component_id, msg, object_detection_event->time, object_detection_event->object_id, object_detection_event->type, object_detection_event->name, object_detection_event->quality, object_detection_event->bearing, object_detection_event->distance);
+}
+
+/**
+ * @brief Send a object_detection_event message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param time Timestamp in milliseconds since system boot
+ * @param object_id Object ID
+ * @param type Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal
+ * @param name Name of the object as defined by the detector
+ * @param quality Detection quality / confidence. 0: bad, 255: maximum confidence
+ * @param bearing Angle of the object with respect to the body frame in NED coordinates in radians. 0: front
+ * @param distance Ground distance in meters
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_object_detection_event_send(mavlink_channel_t chan, uint32_t time, uint16_t object_id, uint8_t type, const char* name, uint8_t quality, float bearing, float distance)
+{
+	mavlink_message_t msg;
+	mavlink_msg_object_detection_event_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time, object_id, type, name, quality, bearing, distance);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE OBJECT_DETECTION_EVENT UNPACKING
+
+/**
+ * @brief Get field time from object_detection_event message
+ *
+ * @return Timestamp in milliseconds since system boot
+ */
+static inline uint32_t mavlink_msg_object_detection_event_get_time(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload)[0];
+	r.b[2] = (msg->payload)[1];
+	r.b[1] = (msg->payload)[2];
+	r.b[0] = (msg->payload)[3];
+	return (uint32_t)r.i;
+}
+
+/**
+ * @brief Get field object_id from object_detection_event message
+ *
+ * @return Object ID
+ */
+static inline uint16_t mavlink_msg_object_detection_event_get_object_id(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint32_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint32_t))[1];
+	return (uint16_t)r.s;
+}
+
+/**
+ * @brief Get field type from object_detection_event message
+ *
+ * @return Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal
+ */
+static inline uint8_t mavlink_msg_object_detection_event_get_type(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload+sizeof(uint32_t)+sizeof(uint16_t))[0];
+}
+
+/**
+ * @brief Get field name from object_detection_event message
+ *
+ * @return Name of the object as defined by the detector
+ */
+static inline uint16_t mavlink_msg_object_detection_event_get_name(const mavlink_message_t* msg, char* r_data)
+{
+
+	memcpy(r_data, msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t), sizeof(char)*20);
+	return sizeof(char)*20;
+}
+
+/**
+ * @brief Get field quality from object_detection_event message
+ *
+ * @return Detection quality / confidence. 0: bad, 255: maximum confidence
+ */
+static inline uint8_t mavlink_msg_object_detection_event_get_quality(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20)[0];
+}
+
+/**
+ * @brief Get field bearing from object_detection_event message
+ *
+ * @return Angle of the object with respect to the body frame in NED coordinates in radians. 0: front
+ */
+static inline float mavlink_msg_object_detection_event_get_bearing(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field distance from object_detection_event message
+ *
+ * @return Ground distance in meters
+ */
+static inline float mavlink_msg_object_detection_event_get_distance(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(char)*20+sizeof(uint8_t)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Decode a object_detection_event message into a struct
+ *
+ * @param msg The message to decode
+ * @param object_detection_event C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_object_detection_event_decode(const mavlink_message_t* msg, mavlink_object_detection_event_t* object_detection_event)
+{
+	object_detection_event->time = mavlink_msg_object_detection_event_get_time(msg);
+	object_detection_event->object_id = mavlink_msg_object_detection_event_get_object_id(msg);
+	object_detection_event->type = mavlink_msg_object_detection_event_get_type(msg);
+	mavlink_msg_object_detection_event_get_name(msg, object_detection_event->name);
+	object_detection_event->quality = mavlink_msg_object_detection_event_get_quality(msg);
+	object_detection_event->bearing = mavlink_msg_object_detection_event_get_bearing(msg);
+	object_detection_event->distance = mavlink_msg_object_detection_event_get_distance(msg);
+}
diff --git a/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h b/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h
new file mode 100644
index 0000000000000000000000000000000000000000..d0acc22ddc39b29155dcbb9ed59aa8586d794311
--- /dev/null
+++ b/thirdParty/mavlink/include/common/mavlink_msg_optical_flow.h
@@ -0,0 +1,206 @@
+// MESSAGE OPTICAL_FLOW PACKING
+
+#define MAVLINK_MSG_ID_OPTICAL_FLOW 100
+
+typedef struct __mavlink_optical_flow_t 
+{
+	uint64_t time; ///< Timestamp (UNIX)
+	uint8_t sensor_id; ///< Sensor ID
+	int16_t flow_x; ///< Flow in pixels in x-sensor direction
+	int16_t flow_y; ///< Flow in pixels in y-sensor direction
+	uint8_t quality; ///< Optical flow quality / confidence. 0: bad, 255: maximum quality
+	float ground_distance; ///< Ground distance in meters
+
+} mavlink_optical_flow_t;
+
+
+
+/**
+ * @brief Pack a optical_flow message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param time Timestamp (UNIX)
+ * @param sensor_id Sensor ID
+ * @param flow_x Flow in pixels in x-sensor direction
+ * @param flow_y Flow in pixels in y-sensor direction
+ * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
+ * @param ground_distance Ground distance in meters
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW;
+
+	i += put_uint64_t_by_index(time, i, msg->payload); // Timestamp (UNIX)
+	i += put_uint8_t_by_index(sensor_id, i, msg->payload); // Sensor ID
+	i += put_int16_t_by_index(flow_x, i, msg->payload); // Flow in pixels in x-sensor direction
+	i += put_int16_t_by_index(flow_y, i, msg->payload); // Flow in pixels in y-sensor direction
+	i += put_uint8_t_by_index(quality, i, msg->payload); // Optical flow quality / confidence. 0: bad, 255: maximum quality
+	i += put_float_by_index(ground_distance, i, msg->payload); // Ground distance in meters
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a optical_flow message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param time Timestamp (UNIX)
+ * @param sensor_id Sensor ID
+ * @param flow_x Flow in pixels in x-sensor direction
+ * @param flow_y Flow in pixels in y-sensor direction
+ * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
+ * @param ground_distance Ground distance in meters
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_OPTICAL_FLOW;
+
+	i += put_uint64_t_by_index(time, i, msg->payload); // Timestamp (UNIX)
+	i += put_uint8_t_by_index(sensor_id, i, msg->payload); // Sensor ID
+	i += put_int16_t_by_index(flow_x, i, msg->payload); // Flow in pixels in x-sensor direction
+	i += put_int16_t_by_index(flow_y, i, msg->payload); // Flow in pixels in y-sensor direction
+	i += put_uint8_t_by_index(quality, i, msg->payload); // Optical flow quality / confidence. 0: bad, 255: maximum quality
+	i += put_float_by_index(ground_distance, i, msg->payload); // Ground distance in meters
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a optical_flow struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param optical_flow C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow)
+{
+	return mavlink_msg_optical_flow_pack(system_id, component_id, msg, optical_flow->time, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->quality, optical_flow->ground_distance);
+}
+
+/**
+ * @brief Send a optical_flow message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param time Timestamp (UNIX)
+ * @param sensor_id Sensor ID
+ * @param flow_x Flow in pixels in x-sensor direction
+ * @param flow_y Flow in pixels in y-sensor direction
+ * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality
+ * @param ground_distance Ground distance in meters
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, uint8_t quality, float ground_distance)
+{
+	mavlink_message_t msg;
+	mavlink_msg_optical_flow_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time, sensor_id, flow_x, flow_y, quality, ground_distance);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE OPTICAL_FLOW UNPACKING
+
+/**
+ * @brief Get field time from optical_flow message
+ *
+ * @return Timestamp (UNIX)
+ */
+static inline uint64_t mavlink_msg_optical_flow_get_time(const mavlink_message_t* msg)
+{
+	generic_64bit r;
+	r.b[7] = (msg->payload)[0];
+	r.b[6] = (msg->payload)[1];
+	r.b[5] = (msg->payload)[2];
+	r.b[4] = (msg->payload)[3];
+	r.b[3] = (msg->payload)[4];
+	r.b[2] = (msg->payload)[5];
+	r.b[1] = (msg->payload)[6];
+	r.b[0] = (msg->payload)[7];
+	return (uint64_t)r.ll;
+}
+
+/**
+ * @brief Get field sensor_id from optical_flow message
+ *
+ * @return Sensor ID
+ */
+static inline uint8_t mavlink_msg_optical_flow_get_sensor_id(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload+sizeof(uint64_t))[0];
+}
+
+/**
+ * @brief Get field flow_x from optical_flow message
+ *
+ * @return Flow in pixels in x-sensor direction
+ */
+static inline int16_t mavlink_msg_optical_flow_get_flow_x(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field flow_y from optical_flow message
+ *
+ * @return Flow in pixels in y-sensor direction
+ */
+static inline int16_t mavlink_msg_optical_flow_get_flow_y(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int16_t))[1];
+	return (int16_t)r.s;
+}
+
+/**
+ * @brief Get field quality from optical_flow message
+ *
+ * @return Optical flow quality / confidence. 0: bad, 255: maximum quality
+ */
+static inline uint8_t mavlink_msg_optical_flow_get_quality(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t))[0];
+}
+
+/**
+ * @brief Get field ground_distance from optical_flow message
+ *
+ * @return Ground distance in meters
+ */
+static inline float mavlink_msg_optical_flow_get_ground_distance(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint8_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint8_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint8_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(uint8_t))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Decode a optical_flow message into a struct
+ *
+ * @param msg The message to decode
+ * @param optical_flow C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_optical_flow_decode(const mavlink_message_t* msg, mavlink_optical_flow_t* optical_flow)
+{
+	optical_flow->time = mavlink_msg_optical_flow_get_time(msg);
+	optical_flow->sensor_id = mavlink_msg_optical_flow_get_sensor_id(msg);
+	optical_flow->flow_x = mavlink_msg_optical_flow_get_flow_x(msg);
+	optical_flow->flow_y = mavlink_msg_optical_flow_get_flow_y(msg);
+	optical_flow->quality = mavlink_msg_optical_flow_get_quality(msg);
+	optical_flow->ground_distance = mavlink_msg_optical_flow_get_ground_distance(msg);
+}
diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h
new file mode 100644
index 0000000000000000000000000000000000000000..7595857a5e0cfb6f14c61817b24aedbec08e185d
--- /dev/null
+++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h
@@ -0,0 +1,278 @@
+// MESSAGE RC_CHANNELS_OVERRIDE PACKING
+
+#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE 70
+
+typedef struct __mavlink_rc_channels_override_t 
+{
+	uint8_t target_system; ///< System ID
+	uint8_t target_component; ///< Component ID
+	uint16_t chan1_raw; ///< RC channel 1 value, in microseconds
+	uint16_t chan2_raw; ///< RC channel 2 value, in microseconds
+	uint16_t chan3_raw; ///< RC channel 3 value, in microseconds
+	uint16_t chan4_raw; ///< RC channel 4 value, in microseconds
+	uint16_t chan5_raw; ///< RC channel 5 value, in microseconds
+	uint16_t chan6_raw; ///< RC channel 6 value, in microseconds
+	uint16_t chan7_raw; ///< RC channel 7 value, in microseconds
+	uint16_t chan8_raw; ///< RC channel 8 value, in microseconds
+
+} mavlink_rc_channels_override_t;
+
+
+
+/**
+ * @brief Pack a rc_channels_override message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param chan1_raw RC channel 1 value, in microseconds
+ * @param chan2_raw RC channel 2 value, in microseconds
+ * @param chan3_raw RC channel 3 value, in microseconds
+ * @param chan4_raw RC channel 4 value, in microseconds
+ * @param chan5_raw RC channel 5 value, in microseconds
+ * @param chan6_raw RC channel 6 value, in microseconds
+ * @param chan7_raw RC channel 7 value, in microseconds
+ * @param chan8_raw RC channel 8 value, in microseconds
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE;
+
+	i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
+	i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
+	i += put_uint16_t_by_index(chan1_raw, i, msg->payload); // RC channel 1 value, in microseconds
+	i += put_uint16_t_by_index(chan2_raw, i, msg->payload); // RC channel 2 value, in microseconds
+	i += put_uint16_t_by_index(chan3_raw, i, msg->payload); // RC channel 3 value, in microseconds
+	i += put_uint16_t_by_index(chan4_raw, i, msg->payload); // RC channel 4 value, in microseconds
+	i += put_uint16_t_by_index(chan5_raw, i, msg->payload); // RC channel 5 value, in microseconds
+	i += put_uint16_t_by_index(chan6_raw, i, msg->payload); // RC channel 6 value, in microseconds
+	i += put_uint16_t_by_index(chan7_raw, i, msg->payload); // RC channel 7 value, in microseconds
+	i += put_uint16_t_by_index(chan8_raw, i, msg->payload); // RC channel 8 value, in microseconds
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a rc_channels_override message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param chan1_raw RC channel 1 value, in microseconds
+ * @param chan2_raw RC channel 2 value, in microseconds
+ * @param chan3_raw RC channel 3 value, in microseconds
+ * @param chan4_raw RC channel 4 value, in microseconds
+ * @param chan5_raw RC channel 5 value, in microseconds
+ * @param chan6_raw RC channel 6 value, in microseconds
+ * @param chan7_raw RC channel 7 value, in microseconds
+ * @param chan8_raw RC channel 8 value, in microseconds
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE;
+
+	i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
+	i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
+	i += put_uint16_t_by_index(chan1_raw, i, msg->payload); // RC channel 1 value, in microseconds
+	i += put_uint16_t_by_index(chan2_raw, i, msg->payload); // RC channel 2 value, in microseconds
+	i += put_uint16_t_by_index(chan3_raw, i, msg->payload); // RC channel 3 value, in microseconds
+	i += put_uint16_t_by_index(chan4_raw, i, msg->payload); // RC channel 4 value, in microseconds
+	i += put_uint16_t_by_index(chan5_raw, i, msg->payload); // RC channel 5 value, in microseconds
+	i += put_uint16_t_by_index(chan6_raw, i, msg->payload); // RC channel 6 value, in microseconds
+	i += put_uint16_t_by_index(chan7_raw, i, msg->payload); // RC channel 7 value, in microseconds
+	i += put_uint16_t_by_index(chan8_raw, i, msg->payload); // RC channel 8 value, in microseconds
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a rc_channels_override struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param rc_channels_override C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override)
+{
+	return mavlink_msg_rc_channels_override_pack(system_id, component_id, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw);
+}
+
+/**
+ * @brief Send a rc_channels_override message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param chan1_raw RC channel 1 value, in microseconds
+ * @param chan2_raw RC channel 2 value, in microseconds
+ * @param chan3_raw RC channel 3 value, in microseconds
+ * @param chan4_raw RC channel 4 value, in microseconds
+ * @param chan5_raw RC channel 5 value, in microseconds
+ * @param chan6_raw RC channel 6 value, in microseconds
+ * @param chan7_raw RC channel 7 value, in microseconds
+ * @param chan8_raw RC channel 8 value, in microseconds
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw)
+{
+	mavlink_message_t msg;
+	mavlink_msg_rc_channels_override_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE RC_CHANNELS_OVERRIDE UNPACKING
+
+/**
+ * @brief Get field target_system from rc_channels_override message
+ *
+ * @return System ID
+ */
+static inline uint8_t mavlink_msg_rc_channels_override_get_target_system(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload)[0];
+}
+
+/**
+ * @brief Get field target_component from rc_channels_override message
+ *
+ * @return Component ID
+ */
+static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload+sizeof(uint8_t))[0];
+}
+
+/**
+ * @brief Get field chan1_raw from rc_channels_override message
+ *
+ * @return RC channel 1 value, in microseconds
+ */
+static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1];
+	return (uint16_t)r.s;
+}
+
+/**
+ * @brief Get field chan2_raw from rc_channels_override message
+ *
+ * @return RC channel 2 value, in microseconds
+ */
+static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t))[1];
+	return (uint16_t)r.s;
+}
+
+/**
+ * @brief Get field chan3_raw from rc_channels_override message
+ *
+ * @return RC channel 3 value, in microseconds
+ */
+static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t))[1];
+	return (uint16_t)r.s;
+}
+
+/**
+ * @brief Get field chan4_raw from rc_channels_override message
+ *
+ * @return RC channel 4 value, in microseconds
+ */
+static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1];
+	return (uint16_t)r.s;
+}
+
+/**
+ * @brief Get field chan5_raw from rc_channels_override message
+ *
+ * @return RC channel 5 value, in microseconds
+ */
+static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1];
+	return (uint16_t)r.s;
+}
+
+/**
+ * @brief Get field chan6_raw from rc_channels_override message
+ *
+ * @return RC channel 6 value, in microseconds
+ */
+static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1];
+	return (uint16_t)r.s;
+}
+
+/**
+ * @brief Get field chan7_raw from rc_channels_override message
+ *
+ * @return RC channel 7 value, in microseconds
+ */
+static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1];
+	return (uint16_t)r.s;
+}
+
+/**
+ * @brief Get field chan8_raw from rc_channels_override message
+ *
+ * @return RC channel 8 value, in microseconds
+ */
+static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg)
+{
+	generic_16bit r;
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[0];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t))[1];
+	return (uint16_t)r.s;
+}
+
+/**
+ * @brief Decode a rc_channels_override message into a struct
+ *
+ * @param msg The message to decode
+ * @param rc_channels_override C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_rc_channels_override_decode(const mavlink_message_t* msg, mavlink_rc_channels_override_t* rc_channels_override)
+{
+	rc_channels_override->target_system = mavlink_msg_rc_channels_override_get_target_system(msg);
+	rc_channels_override->target_component = mavlink_msg_rc_channels_override_get_target_component(msg);
+	rc_channels_override->chan1_raw = mavlink_msg_rc_channels_override_get_chan1_raw(msg);
+	rc_channels_override->chan2_raw = mavlink_msg_rc_channels_override_get_chan2_raw(msg);
+	rc_channels_override->chan3_raw = mavlink_msg_rc_channels_override_get_chan3_raw(msg);
+	rc_channels_override->chan4_raw = mavlink_msg_rc_channels_override_get_chan4_raw(msg);
+	rc_channels_override->chan5_raw = mavlink_msg_rc_channels_override_get_chan5_raw(msg);
+	rc_channels_override->chan6_raw = mavlink_msg_rc_channels_override_get_chan6_raw(msg);
+	rc_channels_override->chan7_raw = mavlink_msg_rc_channels_override_get_chan7_raw(msg);
+	rc_channels_override->chan8_raw = mavlink_msg_rc_channels_override_get_chan8_raw(msg);
+}
diff --git a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h
index f390e4e370c69002aac857b86f5dbec9bbc716f8..15ee1a6a5fa39867628fe87ea9b3dd09b28f0d84 100644
--- a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h
+++ b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h
@@ -7,7 +7,7 @@ typedef struct __mavlink_request_data_stream_t
 	uint8_t target_system; ///< The target requested to send the message stream.
 	uint8_t target_component; ///< The target requested to send the message stream.
 	uint8_t req_stream_id; ///< The ID of the requested message type
-	uint16_t req_message_rate; ///< The requested interval between two messages of this type
+	uint16_t req_message_rate; ///< Update rate in Hertz
 	uint8_t start_stop; ///< 1 to start sending, 0 to stop sending.
 
 } mavlink_request_data_stream_t;
@@ -23,7 +23,7 @@ typedef struct __mavlink_request_data_stream_t
  * @param target_system The target requested to send the message stream.
  * @param target_component The target requested to send the message stream.
  * @param req_stream_id The ID of the requested message type
- * @param req_message_rate The requested interval between two messages of this type
+ * @param req_message_rate Update rate in Hertz
  * @param start_stop 1 to start sending, 0 to stop sending.
  * @return length of the message in bytes (excluding serial stream start sign)
  */
@@ -35,7 +35,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u
 	i += put_uint8_t_by_index(target_system, i, msg->payload); // The target requested to send the message stream.
 	i += put_uint8_t_by_index(target_component, i, msg->payload); // The target requested to send the message stream.
 	i += put_uint8_t_by_index(req_stream_id, i, msg->payload); // The ID of the requested message type
-	i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // The requested interval between two messages of this type
+	i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // Update rate in Hertz
 	i += put_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending.
 
 	return mavlink_finalize_message(msg, system_id, component_id, i);
@@ -50,7 +50,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u
  * @param target_system The target requested to send the message stream.
  * @param target_component The target requested to send the message stream.
  * @param req_stream_id The ID of the requested message type
- * @param req_message_rate The requested interval between two messages of this type
+ * @param req_message_rate Update rate in Hertz
  * @param start_stop 1 to start sending, 0 to stop sending.
  * @return length of the message in bytes (excluding serial stream start sign)
  */
@@ -62,7 +62,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_
 	i += put_uint8_t_by_index(target_system, i, msg->payload); // The target requested to send the message stream.
 	i += put_uint8_t_by_index(target_component, i, msg->payload); // The target requested to send the message stream.
 	i += put_uint8_t_by_index(req_stream_id, i, msg->payload); // The ID of the requested message type
-	i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // The requested interval between two messages of this type
+	i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // Update rate in Hertz
 	i += put_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending.
 
 	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
@@ -88,7 +88,7 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id,
  * @param target_system The target requested to send the message stream.
  * @param target_component The target requested to send the message stream.
  * @param req_stream_id The ID of the requested message type
- * @param req_message_rate The requested interval between two messages of this type
+ * @param req_message_rate Update rate in Hertz
  * @param start_stop 1 to start sending, 0 to stop sending.
  */
 #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
@@ -136,7 +136,7 @@ static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const ma
 /**
  * @brief Get field req_message_rate from request_data_stream message
  *
- * @return The requested interval between two messages of this type
+ * @return Update rate in Hertz
  */
 static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(const mavlink_message_t* msg)
 {
diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h
new file mode 100644
index 0000000000000000000000000000000000000000..ac7a85de3bfe3691f6894af34b8cd23e2da1f8e8
--- /dev/null
+++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h
@@ -0,0 +1,198 @@
+// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT PACKING
+
+#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT 58
+
+typedef struct __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t 
+{
+	uint64_t time_us; ///< Timestamp in micro seconds since unix epoch
+	float roll_speed; ///< Desired roll angular speed in rad/s
+	float pitch_speed; ///< Desired pitch angular speed in rad/s
+	float yaw_speed; ///< Desired yaw angular speed in rad/s
+	float thrust; ///< Collective thrust, normalized to 0 .. 1
+
+} mavlink_roll_pitch_yaw_speed_thrust_setpoint_t;
+
+
+
+/**
+ * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param time_us Timestamp in micro seconds since unix epoch
+ * @param roll_speed Desired roll angular speed in rad/s
+ * @param pitch_speed Desired pitch angular speed in rad/s
+ * @param yaw_speed Desired yaw angular speed in rad/s
+ * @param thrust Collective thrust, normalized to 0 .. 1
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_us, float roll_speed, float pitch_speed, float yaw_speed, float thrust)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT;
+
+	i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp in micro seconds since unix epoch
+	i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s
+	i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s
+	i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s
+	i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a roll_pitch_yaw_speed_thrust_setpoint message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param time_us Timestamp in micro seconds since unix epoch
+ * @param roll_speed Desired roll angular speed in rad/s
+ * @param pitch_speed Desired pitch angular speed in rad/s
+ * @param yaw_speed Desired yaw angular speed in rad/s
+ * @param thrust Collective thrust, normalized to 0 .. 1
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time_us, float roll_speed, float pitch_speed, float yaw_speed, float thrust)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT;
+
+	i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp in micro seconds since unix epoch
+	i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s
+	i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s
+	i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s
+	i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a roll_pitch_yaw_speed_thrust_setpoint struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint)
+{
+	return mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_speed_thrust_setpoint->time_us, roll_pitch_yaw_speed_thrust_setpoint->roll_speed, roll_pitch_yaw_speed_thrust_setpoint->pitch_speed, roll_pitch_yaw_speed_thrust_setpoint->yaw_speed, roll_pitch_yaw_speed_thrust_setpoint->thrust);
+}
+
+/**
+ * @brief Send a roll_pitch_yaw_speed_thrust_setpoint message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param time_us Timestamp in micro seconds since unix epoch
+ * @param roll_speed Desired roll angular speed in rad/s
+ * @param pitch_speed Desired pitch angular speed in rad/s
+ * @param yaw_speed Desired yaw angular speed in rad/s
+ * @param thrust Collective thrust, normalized to 0 .. 1
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_send(mavlink_channel_t chan, uint64_t time_us, float roll_speed, float pitch_speed, float yaw_speed, float thrust)
+{
+	mavlink_message_t msg;
+	mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time_us, roll_speed, pitch_speed, yaw_speed, thrust);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT UNPACKING
+
+/**
+ * @brief Get field time_us from roll_pitch_yaw_speed_thrust_setpoint message
+ *
+ * @return Timestamp in micro seconds since unix epoch
+ */
+static inline uint64_t mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_us(const mavlink_message_t* msg)
+{
+	generic_64bit r;
+	r.b[7] = (msg->payload)[0];
+	r.b[6] = (msg->payload)[1];
+	r.b[5] = (msg->payload)[2];
+	r.b[4] = (msg->payload)[3];
+	r.b[3] = (msg->payload)[4];
+	r.b[2] = (msg->payload)[5];
+	r.b[1] = (msg->payload)[6];
+	r.b[0] = (msg->payload)[7];
+	return (uint64_t)r.ll;
+}
+
+/**
+ * @brief Get field roll_speed from roll_pitch_yaw_speed_thrust_setpoint message
+ *
+ * @return Desired roll angular speed in rad/s
+ */
+static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field pitch_speed from roll_pitch_yaw_speed_thrust_setpoint message
+ *
+ * @return Desired pitch angular speed in rad/s
+ */
+static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field yaw_speed from roll_pitch_yaw_speed_thrust_setpoint message
+ *
+ * @return Desired yaw angular speed in rad/s
+ */
+static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field thrust from roll_pitch_yaw_speed_thrust_setpoint message
+ *
+ * @return Collective thrust, normalized to 0 .. 1
+ */
+static inline float mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Decode a roll_pitch_yaw_speed_thrust_setpoint message into a struct
+ *
+ * @param msg The message to decode
+ * @param roll_pitch_yaw_speed_thrust_setpoint C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_speed_thrust_setpoint_t* roll_pitch_yaw_speed_thrust_setpoint)
+{
+	roll_pitch_yaw_speed_thrust_setpoint->time_us = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_time_us(msg);
+	roll_pitch_yaw_speed_thrust_setpoint->roll_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_roll_speed(msg);
+	roll_pitch_yaw_speed_thrust_setpoint->pitch_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_pitch_speed(msg);
+	roll_pitch_yaw_speed_thrust_setpoint->yaw_speed = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_yaw_speed(msg);
+	roll_pitch_yaw_speed_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint_get_thrust(msg);
+}
diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h
new file mode 100644
index 0000000000000000000000000000000000000000..cc944642748d251f3f9c976a2d1d1550cc816019
--- /dev/null
+++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_thrust_setpoint.h
@@ -0,0 +1,198 @@
+// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT PACKING
+
+#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT 57
+
+typedef struct __mavlink_roll_pitch_yaw_thrust_setpoint_t 
+{
+	uint64_t time_us; ///< Timestamp in micro seconds since unix epoch
+	float roll; ///< Desired roll angle in radians
+	float pitch; ///< Desired pitch angle in radians
+	float yaw; ///< Desired yaw angle in radians
+	float thrust; ///< Collective thrust, normalized to 0 .. 1
+
+} mavlink_roll_pitch_yaw_thrust_setpoint_t;
+
+
+
+/**
+ * @brief Pack a roll_pitch_yaw_thrust_setpoint message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param time_us Timestamp in micro seconds since unix epoch
+ * @param roll Desired roll angle in radians
+ * @param pitch Desired pitch angle in radians
+ * @param yaw Desired yaw angle in radians
+ * @param thrust Collective thrust, normalized to 0 .. 1
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t time_us, float roll, float pitch, float yaw, float thrust)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
+
+	i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp in micro seconds since unix epoch
+	i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians
+	i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians
+	i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians
+	i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a roll_pitch_yaw_thrust_setpoint message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param time_us Timestamp in micro seconds since unix epoch
+ * @param roll Desired roll angle in radians
+ * @param pitch Desired pitch angle in radians
+ * @param yaw Desired yaw angle in radians
+ * @param thrust Collective thrust, normalized to 0 .. 1
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t time_us, float roll, float pitch, float yaw, float thrust)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
+
+	i += put_uint64_t_by_index(time_us, i, msg->payload); // Timestamp in micro seconds since unix epoch
+	i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians
+	i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians
+	i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians
+	i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a roll_pitch_yaw_thrust_setpoint struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param roll_pitch_yaw_thrust_setpoint C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint)
+{
+	return mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_thrust_setpoint->time_us, roll_pitch_yaw_thrust_setpoint->roll, roll_pitch_yaw_thrust_setpoint->pitch, roll_pitch_yaw_thrust_setpoint->yaw, roll_pitch_yaw_thrust_setpoint->thrust);
+}
+
+/**
+ * @brief Send a roll_pitch_yaw_thrust_setpoint message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param time_us Timestamp in micro seconds since unix epoch
+ * @param roll Desired roll angle in radians
+ * @param pitch Desired pitch angle in radians
+ * @param yaw Desired yaw angle in radians
+ * @param thrust Collective thrust, normalized to 0 .. 1
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(mavlink_channel_t chan, uint64_t time_us, float roll, float pitch, float yaw, float thrust)
+{
+	mavlink_message_t msg;
+	mavlink_msg_roll_pitch_yaw_thrust_setpoint_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, time_us, roll, pitch, yaw, thrust);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE ROLL_PITCH_YAW_THRUST_SETPOINT UNPACKING
+
+/**
+ * @brief Get field time_us from roll_pitch_yaw_thrust_setpoint message
+ *
+ * @return Timestamp in micro seconds since unix epoch
+ */
+static inline uint64_t mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_us(const mavlink_message_t* msg)
+{
+	generic_64bit r;
+	r.b[7] = (msg->payload)[0];
+	r.b[6] = (msg->payload)[1];
+	r.b[5] = (msg->payload)[2];
+	r.b[4] = (msg->payload)[3];
+	r.b[3] = (msg->payload)[4];
+	r.b[2] = (msg->payload)[5];
+	r.b[1] = (msg->payload)[6];
+	r.b[0] = (msg->payload)[7];
+	return (uint64_t)r.ll;
+}
+
+/**
+ * @brief Get field roll from roll_pitch_yaw_thrust_setpoint message
+ *
+ * @return Desired roll angle in radians
+ */
+static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field pitch from roll_pitch_yaw_thrust_setpoint message
+ *
+ * @return Desired pitch angle in radians
+ */
+static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field yaw from roll_pitch_yaw_thrust_setpoint message
+ *
+ * @return Desired yaw angle in radians
+ */
+static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field thrust from roll_pitch_yaw_thrust_setpoint message
+ *
+ * @return Collective thrust, normalized to 0 .. 1
+ */
+static inline float mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Decode a roll_pitch_yaw_thrust_setpoint message into a struct
+ *
+ * @param msg The message to decode
+ * @param roll_pitch_yaw_thrust_setpoint C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_roll_pitch_yaw_thrust_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_thrust_setpoint_t* roll_pitch_yaw_thrust_setpoint)
+{
+	roll_pitch_yaw_thrust_setpoint->time_us = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_time_us(msg);
+	roll_pitch_yaw_thrust_setpoint->roll = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_roll(msg);
+	roll_pitch_yaw_thrust_setpoint->pitch = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_pitch(msg);
+	roll_pitch_yaw_thrust_setpoint->yaw = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_yaw(msg);
+	roll_pitch_yaw_thrust_setpoint->thrust = mavlink_msg_roll_pitch_yaw_thrust_setpoint_get_thrust(msg);
+}
diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h
new file mode 100644
index 0000000000000000000000000000000000000000..1cc68dff921cd42edf3eb0a7a383a9821637087a
--- /dev/null
+++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h
@@ -0,0 +1,184 @@
+// MESSAGE SET_ROLL_PITCH_YAW PACKING
+
+#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW 55
+
+typedef struct __mavlink_set_roll_pitch_yaw_t 
+{
+	uint8_t target_system; ///< System ID
+	uint8_t target_component; ///< Component ID
+	float roll; ///< Desired roll angle in radians
+	float pitch; ///< Desired pitch angle in radians
+	float yaw; ///< Desired yaw angle in radians
+
+} mavlink_set_roll_pitch_yaw_t;
+
+
+
+/**
+ * @brief Pack a set_roll_pitch_yaw message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param roll Desired roll angle in radians
+ * @param pitch Desired pitch angle in radians
+ * @param yaw Desired yaw angle in radians
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_set_roll_pitch_yaw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW;
+
+	i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
+	i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
+	i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians
+	i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians
+	i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a set_roll_pitch_yaw message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param roll Desired roll angle in radians
+ * @param pitch Desired pitch angle in radians
+ * @param yaw Desired yaw angle in radians
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_set_roll_pitch_yaw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW;
+
+	i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
+	i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
+	i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians
+	i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians
+	i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a set_roll_pitch_yaw struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param set_roll_pitch_yaw C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_set_roll_pitch_yaw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_t* set_roll_pitch_yaw)
+{
+	return mavlink_msg_set_roll_pitch_yaw_pack(system_id, component_id, msg, set_roll_pitch_yaw->target_system, set_roll_pitch_yaw->target_component, set_roll_pitch_yaw->roll, set_roll_pitch_yaw->pitch, set_roll_pitch_yaw->yaw);
+}
+
+/**
+ * @brief Send a set_roll_pitch_yaw message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param roll Desired roll angle in radians
+ * @param pitch Desired pitch angle in radians
+ * @param yaw Desired yaw angle in radians
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_set_roll_pitch_yaw_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw)
+{
+	mavlink_message_t msg;
+	mavlink_msg_set_roll_pitch_yaw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll, pitch, yaw);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE SET_ROLL_PITCH_YAW UNPACKING
+
+/**
+ * @brief Get field target_system from set_roll_pitch_yaw message
+ *
+ * @return System ID
+ */
+static inline uint8_t mavlink_msg_set_roll_pitch_yaw_get_target_system(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload)[0];
+}
+
+/**
+ * @brief Get field target_component from set_roll_pitch_yaw message
+ *
+ * @return Component ID
+ */
+static inline uint8_t mavlink_msg_set_roll_pitch_yaw_get_target_component(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload+sizeof(uint8_t))[0];
+}
+
+/**
+ * @brief Get field roll from set_roll_pitch_yaw message
+ *
+ * @return Desired roll angle in radians
+ */
+static inline float mavlink_msg_set_roll_pitch_yaw_get_roll(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field pitch from set_roll_pitch_yaw message
+ *
+ * @return Desired pitch angle in radians
+ */
+static inline float mavlink_msg_set_roll_pitch_yaw_get_pitch(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field yaw from set_roll_pitch_yaw message
+ *
+ * @return Desired yaw angle in radians
+ */
+static inline float mavlink_msg_set_roll_pitch_yaw_get_yaw(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Decode a set_roll_pitch_yaw message into a struct
+ *
+ * @param msg The message to decode
+ * @param set_roll_pitch_yaw C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_set_roll_pitch_yaw_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_t* set_roll_pitch_yaw)
+{
+	set_roll_pitch_yaw->target_system = mavlink_msg_set_roll_pitch_yaw_get_target_system(msg);
+	set_roll_pitch_yaw->target_component = mavlink_msg_set_roll_pitch_yaw_get_target_component(msg);
+	set_roll_pitch_yaw->roll = mavlink_msg_set_roll_pitch_yaw_get_roll(msg);
+	set_roll_pitch_yaw->pitch = mavlink_msg_set_roll_pitch_yaw_get_pitch(msg);
+	set_roll_pitch_yaw->yaw = mavlink_msg_set_roll_pitch_yaw_get_yaw(msg);
+}
diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h
new file mode 100644
index 0000000000000000000000000000000000000000..7855daf765a2e176d7020e1d23295e9351c22083
--- /dev/null
+++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h
@@ -0,0 +1,184 @@
+// MESSAGE SET_ROLL_PITCH_YAW_SPEED PACKING
+
+#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED 56
+
+typedef struct __mavlink_set_roll_pitch_yaw_speed_t 
+{
+	uint8_t target_system; ///< System ID
+	uint8_t target_component; ///< Component ID
+	float roll_speed; ///< Desired roll angular speed in rad/s
+	float pitch_speed; ///< Desired pitch angular speed in rad/s
+	float yaw_speed; ///< Desired yaw angular speed in rad/s
+
+} mavlink_set_roll_pitch_yaw_speed_t;
+
+
+
+/**
+ * @brief Pack a set_roll_pitch_yaw_speed message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param roll_speed Desired roll angular speed in rad/s
+ * @param pitch_speed Desired pitch angular speed in rad/s
+ * @param yaw_speed Desired yaw angular speed in rad/s
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED;
+
+	i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
+	i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
+	i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s
+	i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s
+	i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a set_roll_pitch_yaw_speed message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param roll_speed Desired roll angular speed in rad/s
+ * @param pitch_speed Desired pitch angular speed in rad/s
+ * @param yaw_speed Desired yaw angular speed in rad/s
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED;
+
+	i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
+	i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
+	i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s
+	i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s
+	i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a set_roll_pitch_yaw_speed struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param set_roll_pitch_yaw_speed C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed)
+{
+	return mavlink_msg_set_roll_pitch_yaw_speed_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed->target_system, set_roll_pitch_yaw_speed->target_component, set_roll_pitch_yaw_speed->roll_speed, set_roll_pitch_yaw_speed->pitch_speed, set_roll_pitch_yaw_speed->yaw_speed);
+}
+
+/**
+ * @brief Send a set_roll_pitch_yaw_speed message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param roll_speed Desired roll angular speed in rad/s
+ * @param pitch_speed Desired pitch angular speed in rad/s
+ * @param yaw_speed Desired yaw angular speed in rad/s
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_set_roll_pitch_yaw_speed_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed)
+{
+	mavlink_message_t msg;
+	mavlink_msg_set_roll_pitch_yaw_speed_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll_speed, pitch_speed, yaw_speed);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE SET_ROLL_PITCH_YAW_SPEED UNPACKING
+
+/**
+ * @brief Get field target_system from set_roll_pitch_yaw_speed message
+ *
+ * @return System ID
+ */
+static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_system(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload)[0];
+}
+
+/**
+ * @brief Get field target_component from set_roll_pitch_yaw_speed message
+ *
+ * @return Component ID
+ */
+static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_component(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload+sizeof(uint8_t))[0];
+}
+
+/**
+ * @brief Get field roll_speed from set_roll_pitch_yaw_speed message
+ *
+ * @return Desired roll angular speed in rad/s
+ */
+static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_roll_speed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field pitch_speed from set_roll_pitch_yaw_speed message
+ *
+ * @return Desired pitch angular speed in rad/s
+ */
+static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_pitch_speed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field yaw_speed from set_roll_pitch_yaw_speed message
+ *
+ * @return Desired yaw angular speed in rad/s
+ */
+static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_yaw_speed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Decode a set_roll_pitch_yaw_speed message into a struct
+ *
+ * @param msg The message to decode
+ * @param set_roll_pitch_yaw_speed C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_set_roll_pitch_yaw_speed_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed)
+{
+	set_roll_pitch_yaw_speed->target_system = mavlink_msg_set_roll_pitch_yaw_speed_get_target_system(msg);
+	set_roll_pitch_yaw_speed->target_component = mavlink_msg_set_roll_pitch_yaw_speed_get_target_component(msg);
+	set_roll_pitch_yaw_speed->roll_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_roll_speed(msg);
+	set_roll_pitch_yaw_speed->pitch_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_pitch_speed(msg);
+	set_roll_pitch_yaw_speed->yaw_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_yaw_speed(msg);
+}
diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd12ea4cb09bc882873096c2ecb606019c2e3f31
--- /dev/null
+++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed_thrust.h
@@ -0,0 +1,206 @@
+// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST PACKING
+
+#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST 56
+
+typedef struct __mavlink_set_roll_pitch_yaw_speed_thrust_t 
+{
+	uint8_t target_system; ///< System ID
+	uint8_t target_component; ///< Component ID
+	float roll_speed; ///< Desired roll angular speed in rad/s
+	float pitch_speed; ///< Desired pitch angular speed in rad/s
+	float yaw_speed; ///< Desired yaw angular speed in rad/s
+	float thrust; ///< Collective thrust, normalized to 0 .. 1
+
+} mavlink_set_roll_pitch_yaw_speed_thrust_t;
+
+
+
+/**
+ * @brief Pack a set_roll_pitch_yaw_speed_thrust message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param roll_speed Desired roll angular speed in rad/s
+ * @param pitch_speed Desired pitch angular speed in rad/s
+ * @param yaw_speed Desired yaw angular speed in rad/s
+ * @param thrust Collective thrust, normalized to 0 .. 1
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST;
+
+	i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
+	i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
+	i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s
+	i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s
+	i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s
+	i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a set_roll_pitch_yaw_speed_thrust message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param roll_speed Desired roll angular speed in rad/s
+ * @param pitch_speed Desired pitch angular speed in rad/s
+ * @param yaw_speed Desired yaw angular speed in rad/s
+ * @param thrust Collective thrust, normalized to 0 .. 1
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST;
+
+	i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
+	i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
+	i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s
+	i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s
+	i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s
+	i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a set_roll_pitch_yaw_speed_thrust struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param set_roll_pitch_yaw_speed_thrust C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust)
+{
+	return mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed_thrust->target_system, set_roll_pitch_yaw_speed_thrust->target_component, set_roll_pitch_yaw_speed_thrust->roll_speed, set_roll_pitch_yaw_speed_thrust->pitch_speed, set_roll_pitch_yaw_speed_thrust->yaw_speed, set_roll_pitch_yaw_speed_thrust->thrust);
+}
+
+/**
+ * @brief Send a set_roll_pitch_yaw_speed_thrust message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param roll_speed Desired roll angular speed in rad/s
+ * @param pitch_speed Desired pitch angular speed in rad/s
+ * @param yaw_speed Desired yaw angular speed in rad/s
+ * @param thrust Collective thrust, normalized to 0 .. 1
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed, float thrust)
+{
+	mavlink_message_t msg;
+	mavlink_msg_set_roll_pitch_yaw_speed_thrust_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE SET_ROLL_PITCH_YAW_SPEED_THRUST UNPACKING
+
+/**
+ * @brief Get field target_system from set_roll_pitch_yaw_speed_thrust message
+ *
+ * @return System ID
+ */
+static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload)[0];
+}
+
+/**
+ * @brief Get field target_component from set_roll_pitch_yaw_speed_thrust message
+ *
+ * @return Component ID
+ */
+static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload+sizeof(uint8_t))[0];
+}
+
+/**
+ * @brief Get field roll_speed from set_roll_pitch_yaw_speed_thrust message
+ *
+ * @return Desired roll angular speed in rad/s
+ */
+static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field pitch_speed from set_roll_pitch_yaw_speed_thrust message
+ *
+ * @return Desired pitch angular speed in rad/s
+ */
+static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field yaw_speed from set_roll_pitch_yaw_speed_thrust message
+ *
+ * @return Desired yaw angular speed in rad/s
+ */
+static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field thrust from set_roll_pitch_yaw_speed_thrust message
+ *
+ * @return Collective thrust, normalized to 0 .. 1
+ */
+static inline float mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Decode a set_roll_pitch_yaw_speed_thrust message into a struct
+ *
+ * @param msg The message to decode
+ * @param set_roll_pitch_yaw_speed_thrust C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_set_roll_pitch_yaw_speed_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_thrust_t* set_roll_pitch_yaw_speed_thrust)
+{
+	set_roll_pitch_yaw_speed_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_system(msg);
+	set_roll_pitch_yaw_speed_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_target_component(msg);
+	set_roll_pitch_yaw_speed_thrust->roll_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_roll_speed(msg);
+	set_roll_pitch_yaw_speed_thrust->pitch_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_pitch_speed(msg);
+	set_roll_pitch_yaw_speed_thrust->yaw_speed = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_yaw_speed(msg);
+	set_roll_pitch_yaw_speed_thrust->thrust = mavlink_msg_set_roll_pitch_yaw_speed_thrust_get_thrust(msg);
+}
diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h
new file mode 100644
index 0000000000000000000000000000000000000000..7bf7d26247de458a5c10f7600da0358d7924e659
--- /dev/null
+++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_thrust.h
@@ -0,0 +1,206 @@
+// MESSAGE SET_ROLL_PITCH_YAW_THRUST PACKING
+
+#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST 55
+
+typedef struct __mavlink_set_roll_pitch_yaw_thrust_t 
+{
+	uint8_t target_system; ///< System ID
+	uint8_t target_component; ///< Component ID
+	float roll; ///< Desired roll angle in radians
+	float pitch; ///< Desired pitch angle in radians
+	float yaw; ///< Desired yaw angle in radians
+	float thrust; ///< Collective thrust, normalized to 0 .. 1
+
+} mavlink_set_roll_pitch_yaw_thrust_t;
+
+
+
+/**
+ * @brief Pack a set_roll_pitch_yaw_thrust message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param roll Desired roll angle in radians
+ * @param pitch Desired pitch angle in radians
+ * @param yaw Desired yaw angle in radians
+ * @param thrust Collective thrust, normalized to 0 .. 1
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST;
+
+	i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
+	i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
+	i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians
+	i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians
+	i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians
+	i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a set_roll_pitch_yaw_thrust message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param roll Desired roll angle in radians
+ * @param pitch Desired pitch angle in radians
+ * @param yaw Desired yaw angle in radians
+ * @param thrust Collective thrust, normalized to 0 .. 1
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST;
+
+	i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
+	i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
+	i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians
+	i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians
+	i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians
+	i += put_float_by_index(thrust, i, msg->payload); // Collective thrust, normalized to 0 .. 1
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a set_roll_pitch_yaw_thrust struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param set_roll_pitch_yaw_thrust C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_set_roll_pitch_yaw_thrust_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust)
+{
+	return mavlink_msg_set_roll_pitch_yaw_thrust_pack(system_id, component_id, msg, set_roll_pitch_yaw_thrust->target_system, set_roll_pitch_yaw_thrust->target_component, set_roll_pitch_yaw_thrust->roll, set_roll_pitch_yaw_thrust->pitch, set_roll_pitch_yaw_thrust->yaw, set_roll_pitch_yaw_thrust->thrust);
+}
+
+/**
+ * @brief Send a set_roll_pitch_yaw_thrust message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param target_system System ID
+ * @param target_component Component ID
+ * @param roll Desired roll angle in radians
+ * @param pitch Desired pitch angle in radians
+ * @param yaw Desired yaw angle in radians
+ * @param thrust Collective thrust, normalized to 0 .. 1
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_set_roll_pitch_yaw_thrust_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw, float thrust)
+{
+	mavlink_message_t msg;
+	mavlink_msg_set_roll_pitch_yaw_thrust_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll, pitch, yaw, thrust);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE SET_ROLL_PITCH_YAW_THRUST UNPACKING
+
+/**
+ * @brief Get field target_system from set_roll_pitch_yaw_thrust message
+ *
+ * @return System ID
+ */
+static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload)[0];
+}
+
+/**
+ * @brief Get field target_component from set_roll_pitch_yaw_thrust message
+ *
+ * @return Component ID
+ */
+static inline uint8_t mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(const mavlink_message_t* msg)
+{
+	return (uint8_t)(msg->payload+sizeof(uint8_t))[0];
+}
+
+/**
+ * @brief Get field roll from set_roll_pitch_yaw_thrust message
+ *
+ * @return Desired roll angle in radians
+ */
+static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field pitch from set_roll_pitch_yaw_thrust message
+ *
+ * @return Desired pitch angle in radians
+ */
+static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field yaw from set_roll_pitch_yaw_thrust message
+ *
+ * @return Desired yaw angle in radians
+ */
+static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field thrust from set_roll_pitch_yaw_thrust message
+ *
+ * @return Collective thrust, normalized to 0 .. 1
+ */
+static inline float mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Decode a set_roll_pitch_yaw_thrust message into a struct
+ *
+ * @param msg The message to decode
+ * @param set_roll_pitch_yaw_thrust C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_set_roll_pitch_yaw_thrust_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_thrust_t* set_roll_pitch_yaw_thrust)
+{
+	set_roll_pitch_yaw_thrust->target_system = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_system(msg);
+	set_roll_pitch_yaw_thrust->target_component = mavlink_msg_set_roll_pitch_yaw_thrust_get_target_component(msg);
+	set_roll_pitch_yaw_thrust->roll = mavlink_msg_set_roll_pitch_yaw_thrust_get_roll(msg);
+	set_roll_pitch_yaw_thrust->pitch = mavlink_msg_set_roll_pitch_yaw_thrust_get_pitch(msg);
+	set_roll_pitch_yaw_thrust->yaw = mavlink_msg_set_roll_pitch_yaw_thrust_get_yaw(msg);
+	set_roll_pitch_yaw_thrust->thrust = mavlink_msg_set_roll_pitch_yaw_thrust_get_thrust(msg);
+}
diff --git a/thirdParty/mavlink/include/minimal/mavlink.h b/thirdParty/mavlink/include/minimal/mavlink.h
index 45a5e9566a8f66618fc3b88cc4469b032ebabc7b..38389146abe8b01e562eacc67c2e200690b4dde6 100644
--- a/thirdParty/mavlink/include/minimal/mavlink.h
+++ b/thirdParty/mavlink/include/minimal/mavlink.h
@@ -1,7 +1,7 @@
 /** @file
  *	@brief MAVLink comm protocol.
  *	@see http://pixhawk.ethz.ch/software/mavlink
- *	 Generated on Wednesday, July 27 2011, 14:17 UTC
+ *	 Generated on Friday, August 5 2011, 07:37 UTC
  */
 #ifndef MAVLINK_H
 #define MAVLINK_H
diff --git a/thirdParty/mavlink/include/minimal/minimal.h b/thirdParty/mavlink/include/minimal/minimal.h
index 61cd3fe22be39ed8f8bdaaf855398f3d7cce25d2..ee09135a68cf8baf23eaa915b6791e0b1168d99c 100644
--- a/thirdParty/mavlink/include/minimal/minimal.h
+++ b/thirdParty/mavlink/include/minimal/minimal.h
@@ -1,7 +1,7 @@
 /** @file
  *	@brief MAVLink comm protocol.
  *	@see http://qgroundcontrol.org/mavlink/
- *	 Generated on Wednesday, July 27 2011, 14:17 UTC
+ *	 Generated on Friday, August 5 2011, 07:37 UTC
  */
 #ifndef MINIMAL_H
 #define MINIMAL_H
diff --git a/thirdParty/mavlink/include/pixhawk/mavlink.h b/thirdParty/mavlink/include/pixhawk/mavlink.h
index 16a37599e1fa218650bf065dbd66ec620892162c..57a763b36723d0002ed3fc496cbfea74d5f5e5e7 100644
--- a/thirdParty/mavlink/include/pixhawk/mavlink.h
+++ b/thirdParty/mavlink/include/pixhawk/mavlink.h
@@ -1,7 +1,7 @@
 /** @file
  *	@brief MAVLink comm protocol.
  *	@see http://pixhawk.ethz.ch/software/mavlink
- *	 Generated on Wednesday, July 27 2011, 14:17 UTC
+ *	 Generated on Wednesday, August 17 2011, 06:03 UTC
  */
 #ifndef MAVLINK_H
 #define MAVLINK_H
diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_visual_odometry.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_visual_odometry.h
new file mode 100644
index 0000000000000000000000000000000000000000..0fd504c1ff9e9ea7d597ed0ac200744c8fdd9959
--- /dev/null
+++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_visual_odometry.h
@@ -0,0 +1,268 @@
+// MESSAGE VISUAL_ODOMETRY PACKING
+
+#define MAVLINK_MSG_ID_VISUAL_ODOMETRY 180
+
+typedef struct __mavlink_visual_odometry_t 
+{
+	uint64_t frame1_time_us; ///< Time at which frame 1 was captured (in microseconds since unix epoch)
+	uint64_t frame2_time_us; ///< Time at which frame 2 was captured (in microseconds since unix epoch)
+	float x; ///< Relative X position
+	float y; ///< Relative Y position
+	float z; ///< Relative Z position
+	float roll; ///< Relative roll angle in rad
+	float pitch; ///< Relative pitch angle in rad
+	float yaw; ///< Relative yaw angle in rad
+
+} mavlink_visual_odometry_t;
+
+
+
+/**
+ * @brief Pack a visual_odometry message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ *
+ * @param frame1_time_us Time at which frame 1 was captured (in microseconds since unix epoch)
+ * @param frame2_time_us Time at which frame 2 was captured (in microseconds since unix epoch)
+ * @param x Relative X position
+ * @param y Relative Y position
+ * @param z Relative Z position
+ * @param roll Relative roll angle in rad
+ * @param pitch Relative pitch angle in rad
+ * @param yaw Relative yaw angle in rad
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_visual_odometry_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t frame1_time_us, uint64_t frame2_time_us, float x, float y, float z, float roll, float pitch, float yaw)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_VISUAL_ODOMETRY;
+
+	i += put_uint64_t_by_index(frame1_time_us, i, msg->payload); // Time at which frame 1 was captured (in microseconds since unix epoch)
+	i += put_uint64_t_by_index(frame2_time_us, i, msg->payload); // Time at which frame 2 was captured (in microseconds since unix epoch)
+	i += put_float_by_index(x, i, msg->payload); // Relative X position
+	i += put_float_by_index(y, i, msg->payload); // Relative Y position
+	i += put_float_by_index(z, i, msg->payload); // Relative Z position
+	i += put_float_by_index(roll, i, msg->payload); // Relative roll angle in rad
+	i += put_float_by_index(pitch, i, msg->payload); // Relative pitch angle in rad
+	i += put_float_by_index(yaw, i, msg->payload); // Relative yaw angle in rad
+
+	return mavlink_finalize_message(msg, system_id, component_id, i);
+}
+
+/**
+ * @brief Pack a visual_odometry message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message was sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param frame1_time_us Time at which frame 1 was captured (in microseconds since unix epoch)
+ * @param frame2_time_us Time at which frame 2 was captured (in microseconds since unix epoch)
+ * @param x Relative X position
+ * @param y Relative Y position
+ * @param z Relative Z position
+ * @param roll Relative roll angle in rad
+ * @param pitch Relative pitch angle in rad
+ * @param yaw Relative yaw angle in rad
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_visual_odometry_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t frame1_time_us, uint64_t frame2_time_us, float x, float y, float z, float roll, float pitch, float yaw)
+{
+	uint16_t i = 0;
+	msg->msgid = MAVLINK_MSG_ID_VISUAL_ODOMETRY;
+
+	i += put_uint64_t_by_index(frame1_time_us, i, msg->payload); // Time at which frame 1 was captured (in microseconds since unix epoch)
+	i += put_uint64_t_by_index(frame2_time_us, i, msg->payload); // Time at which frame 2 was captured (in microseconds since unix epoch)
+	i += put_float_by_index(x, i, msg->payload); // Relative X position
+	i += put_float_by_index(y, i, msg->payload); // Relative Y position
+	i += put_float_by_index(z, i, msg->payload); // Relative Z position
+	i += put_float_by_index(roll, i, msg->payload); // Relative roll angle in rad
+	i += put_float_by_index(pitch, i, msg->payload); // Relative pitch angle in rad
+	i += put_float_by_index(yaw, i, msg->payload); // Relative yaw angle in rad
+
+	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
+}
+
+/**
+ * @brief Encode a visual_odometry struct into a message
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param msg The MAVLink message to compress the data into
+ * @param visual_odometry C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_visual_odometry_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_visual_odometry_t* visual_odometry)
+{
+	return mavlink_msg_visual_odometry_pack(system_id, component_id, msg, visual_odometry->frame1_time_us, visual_odometry->frame2_time_us, visual_odometry->x, visual_odometry->y, visual_odometry->z, visual_odometry->roll, visual_odometry->pitch, visual_odometry->yaw);
+}
+
+/**
+ * @brief Send a visual_odometry message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param frame1_time_us Time at which frame 1 was captured (in microseconds since unix epoch)
+ * @param frame2_time_us Time at which frame 2 was captured (in microseconds since unix epoch)
+ * @param x Relative X position
+ * @param y Relative Y position
+ * @param z Relative Z position
+ * @param roll Relative roll angle in rad
+ * @param pitch Relative pitch angle in rad
+ * @param yaw Relative yaw angle in rad
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_visual_odometry_send(mavlink_channel_t chan, uint64_t frame1_time_us, uint64_t frame2_time_us, float x, float y, float z, float roll, float pitch, float yaw)
+{
+	mavlink_message_t msg;
+	mavlink_msg_visual_odometry_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, frame1_time_us, frame2_time_us, x, y, z, roll, pitch, yaw);
+	mavlink_send_uart(chan, &msg);
+}
+
+#endif
+// MESSAGE VISUAL_ODOMETRY UNPACKING
+
+/**
+ * @brief Get field frame1_time_us from visual_odometry message
+ *
+ * @return Time at which frame 1 was captured (in microseconds since unix epoch)
+ */
+static inline uint64_t mavlink_msg_visual_odometry_get_frame1_time_us(const mavlink_message_t* msg)
+{
+	generic_64bit r;
+	r.b[7] = (msg->payload)[0];
+	r.b[6] = (msg->payload)[1];
+	r.b[5] = (msg->payload)[2];
+	r.b[4] = (msg->payload)[3];
+	r.b[3] = (msg->payload)[4];
+	r.b[2] = (msg->payload)[5];
+	r.b[1] = (msg->payload)[6];
+	r.b[0] = (msg->payload)[7];
+	return (uint64_t)r.ll;
+}
+
+/**
+ * @brief Get field frame2_time_us from visual_odometry message
+ *
+ * @return Time at which frame 2 was captured (in microseconds since unix epoch)
+ */
+static inline uint64_t mavlink_msg_visual_odometry_get_frame2_time_us(const mavlink_message_t* msg)
+{
+	generic_64bit r;
+	r.b[7] = (msg->payload+sizeof(uint64_t))[0];
+	r.b[6] = (msg->payload+sizeof(uint64_t))[1];
+	r.b[5] = (msg->payload+sizeof(uint64_t))[2];
+	r.b[4] = (msg->payload+sizeof(uint64_t))[3];
+	r.b[3] = (msg->payload+sizeof(uint64_t))[4];
+	r.b[2] = (msg->payload+sizeof(uint64_t))[5];
+	r.b[1] = (msg->payload+sizeof(uint64_t))[6];
+	r.b[0] = (msg->payload+sizeof(uint64_t))[7];
+	return (uint64_t)r.ll;
+}
+
+/**
+ * @brief Get field x from visual_odometry message
+ *
+ * @return Relative X position
+ */
+static inline float mavlink_msg_visual_odometry_get_x(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field y from visual_odometry message
+ *
+ * @return Relative Y position
+ */
+static inline float mavlink_msg_visual_odometry_get_y(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field z from visual_odometry message
+ *
+ * @return Relative Z position
+ */
+static inline float mavlink_msg_visual_odometry_get_z(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field roll from visual_odometry message
+ *
+ * @return Relative roll angle in rad
+ */
+static inline float mavlink_msg_visual_odometry_get_roll(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field pitch from visual_odometry message
+ *
+ * @return Relative pitch angle in rad
+ */
+static inline float mavlink_msg_visual_odometry_get_pitch(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Get field yaw from visual_odometry message
+ *
+ * @return Relative yaw angle in rad
+ */
+static inline float mavlink_msg_visual_odometry_get_yaw(const mavlink_message_t* msg)
+{
+	generic_32bit r;
+	r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
+	r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
+	r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
+	r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
+	return (float)r.f;
+}
+
+/**
+ * @brief Decode a visual_odometry message into a struct
+ *
+ * @param msg The message to decode
+ * @param visual_odometry C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_visual_odometry_decode(const mavlink_message_t* msg, mavlink_visual_odometry_t* visual_odometry)
+{
+	visual_odometry->frame1_time_us = mavlink_msg_visual_odometry_get_frame1_time_us(msg);
+	visual_odometry->frame2_time_us = mavlink_msg_visual_odometry_get_frame2_time_us(msg);
+	visual_odometry->x = mavlink_msg_visual_odometry_get_x(msg);
+	visual_odometry->y = mavlink_msg_visual_odometry_get_y(msg);
+	visual_odometry->z = mavlink_msg_visual_odometry_get_z(msg);
+	visual_odometry->roll = mavlink_msg_visual_odometry_get_roll(msg);
+	visual_odometry->pitch = mavlink_msg_visual_odometry_get_pitch(msg);
+	visual_odometry->yaw = mavlink_msg_visual_odometry_get_yaw(msg);
+}
diff --git a/thirdParty/mavlink/include/pixhawk/pixhawk.h b/thirdParty/mavlink/include/pixhawk/pixhawk.h
index 30474d09cb8c1268d12d14fe373d79f0f06551a4..5907478c2bbd9a1aa449f72da46b39ab44ae1d13 100644
--- a/thirdParty/mavlink/include/pixhawk/pixhawk.h
+++ b/thirdParty/mavlink/include/pixhawk/pixhawk.h
@@ -1,7 +1,7 @@
 /** @file
  *	@brief MAVLink comm protocol.
  *	@see http://qgroundcontrol.org/mavlink/
- *	 Generated on Wednesday, July 27 2011, 14:17 UTC
+ *	 Generated on Wednesday, August 17 2011, 06:03 UTC
  */
 #ifndef PIXHAWK_H
 #define PIXHAWK_H
@@ -66,12 +66,13 @@ enum DATA_TYPES
 #include "./mavlink_msg_data_transmission_handshake.h"
 #include "./mavlink_msg_encapsulated_data.h"
 #include "./mavlink_msg_brief_feature.h"
+#include "./mavlink_msg_visual_odometry.h"
 
 
 // MESSAGE LENGTHS
 
 #undef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 0, 0, 32, 32, 20, 0, 0, 0, 0, 0, 0, 20, 18, 0, 0, 0, 0, 0, 0, 0, 0, 26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 12, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 18, 0, 0, 0, 0, 0, 106, 42, 54, 0, 0, 0, 0, 0, 0, 0, 8, 255, 53, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
+#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 0, 0, 32, 32, 20, 0, 0, 0, 0, 0, 0, 20, 18, 0, 0, 0, 0, 0, 0, 0, 0, 26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 12, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 18, 0, 0, 0, 0, 0, 106, 42, 54, 0, 0, 0, 0, 0, 0, 0, 8, 255, 53, 0, 0, 0, 0, 0, 0, 0, 40, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
 
 #ifdef __cplusplus
 }
diff --git a/thirdParty/mavlink/include/slugs/mavlink.h b/thirdParty/mavlink/include/slugs/mavlink.h
index bf0e146b4c4ac71ac936d8deb85044d42555ee1c..29ce7ed5554b80cf15e4d041ed91ecf44217de84 100644
--- a/thirdParty/mavlink/include/slugs/mavlink.h
+++ b/thirdParty/mavlink/include/slugs/mavlink.h
@@ -1,7 +1,7 @@
 /** @file
  *	@brief MAVLink comm protocol.
  *	@see http://pixhawk.ethz.ch/software/mavlink
- *	 Generated on Wednesday, July 27 2011, 14:17 UTC
+ *	 Generated on Friday, August 5 2011, 07:37 UTC
  */
 #ifndef MAVLINK_H
 #define MAVLINK_H
diff --git a/thirdParty/mavlink/include/slugs/slugs.h b/thirdParty/mavlink/include/slugs/slugs.h
index b8a6ff330ac6767501f59b0c042cd7b9966b3ab3..b04cb7922f08e9aa1733bbd5f934f9be18496b6f 100644
--- a/thirdParty/mavlink/include/slugs/slugs.h
+++ b/thirdParty/mavlink/include/slugs/slugs.h
@@ -1,7 +1,7 @@
 /** @file
  *	@brief MAVLink comm protocol.
  *	@see http://qgroundcontrol.org/mavlink/
- *	 Generated on Wednesday, July 27 2011, 14:17 UTC
+ *	 Generated on Friday, August 5 2011, 07:37 UTC
  */
 #ifndef SLUGS_H
 #define SLUGS_H
@@ -48,7 +48,7 @@ extern "C" {
 // MESSAGE LENGTHS
 
 #undef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 10, 24, 18, 0, 0, 30, 24, 0, 7, 13, 3, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
+#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 14, 14, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 4, 10, 24, 18, 0, 0, 30, 24, 0, 7, 13, 3, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
 
 #ifdef __cplusplus
 }
diff --git a/thirdParty/mavlink/include/ualberta/mavlink.h b/thirdParty/mavlink/include/ualberta/mavlink.h
index 30b060f630050f4b68a710e97ca0c3eed2ca0e9b..e12f74a8df8d6811ffda3cbec7ed6dc2f040dbae 100644
--- a/thirdParty/mavlink/include/ualberta/mavlink.h
+++ b/thirdParty/mavlink/include/ualberta/mavlink.h
@@ -1,7 +1,7 @@
 /** @file
  *	@brief MAVLink comm protocol.
  *	@see http://pixhawk.ethz.ch/software/mavlink
- *	 Generated on Wednesday, July 27 2011, 14:17 UTC
+ *	 Generated on Friday, August 5 2011, 07:37 UTC
  */
 #ifndef MAVLINK_H
 #define MAVLINK_H
diff --git a/thirdParty/mavlink/include/ualberta/ualberta.h b/thirdParty/mavlink/include/ualberta/ualberta.h
index b492ff62d65eead6b12bd73363dcbb4b22791ae2..be77cc2e5d7e6b898d2ec61f8f1760fa65a9c575 100644
--- a/thirdParty/mavlink/include/ualberta/ualberta.h
+++ b/thirdParty/mavlink/include/ualberta/ualberta.h
@@ -1,7 +1,7 @@
 /** @file
  *	@brief MAVLink comm protocol.
  *	@see http://qgroundcontrol.org/mavlink/
- *	 Generated on Wednesday, July 27 2011, 14:17 UTC
+ *	 Generated on Friday, August 5 2011, 07:37 UTC
  */
 #ifndef UALBERTA_H
 #define UALBERTA_H
@@ -71,7 +71,7 @@ enum UALBERTA_PILOT_MODE
 // MESSAGE LENGTHS
 
 #undef MAVLINK_MESSAGE_LENGTHS
-#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32, 42, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
+#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 14, 14, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 32, 42, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
 
 #ifdef __cplusplus
 }
diff --git a/thirdParty/mavlink/message_definitions/ardupilotmega.xml b/thirdParty/mavlink/message_definitions/ardupilotmega.xml
index 39d96eab515cf9fb19b6fdf0bf167e369be00a50..d7d6aaacd414de6f984013295fea2e3910d95bea 100644
--- a/thirdParty/mavlink/message_definitions/ardupilotmega.xml
+++ b/thirdParty/mavlink/message_definitions/ardupilotmega.xml
@@ -1,6 +1,39 @@
-<?xml version="1.0"?>
+<?xml version='1.0'?>
 <mavlink>
-	<include>common.xml</include>
-<messages>
-</messages>
+     <include>common.xml</include>
+     <!-- note that APM specific messages should use the command id
+      range from 150 to 250, to leave plenty of room for growth
+      of common.xml 
+
+      If you prototype a message here, then you should consider if it
+      is general enough to move into common.xml later
+    -->
+     <messages>
+          <message id="150" name="SENSOR_OFFSETS">
+               <description>Offsets and calibrations values for hardware
+        sensors. This makes it easier to debug the calibration process.</description>
+               <field type="int16_t" name="mag_ofs_x">magnetometer X offset</field>
+               <field type="int16_t" name="mag_ofs_y">magnetometer Y offset</field>
+               <field type="int16_t" name="mag_ofs_z">magnetometer Z offset</field>
+               <field type="float" name="mag_declination">magnetic declination (radians)</field>
+               <field type="int32_t" name="raw_press">raw pressure from barometer</field>
+               <field type="int32_t" name="raw_temp">raw temperature from barometer</field>
+               <field type="float" name="gyro_cal_x">gyro X calibration</field>
+               <field type="float" name="gyro_cal_y">gyro Y calibration</field>
+               <field type="float" name="gyro_cal_z">gyro Z calibration</field>
+               <field type="float" name="accel_cal_x">accel X calibration</field>
+               <field type="float" name="accel_cal_y">accel Y calibration</field>
+               <field type="float" name="accel_cal_z">accel Z calibration</field>
+          </message>
+
+          <message id="151" name="SET_MAG_OFFSETS">
+               <description>set the magnetometer offsets</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+               <field type="int16_t" name="mag_ofs_x">magnetometer X offset</field>
+               <field type="int16_t" name="mag_ofs_y">magnetometer Y offset</field>
+               <field type="int16_t" name="mag_ofs_z">magnetometer Z offset</field>
+          </message>
+
+     </messages>
 </mavlink>
diff --git a/thirdParty/mavlink/message_definitions/common.xml b/thirdParty/mavlink/message_definitions/common.xml
index 5df9ba827ee9112b0755dd4ece6918f48a71bbb7..dd4ea84430da5a7ceed5ae92bdfaaff54621d75c 100644
--- a/thirdParty/mavlink/message_definitions/common.xml
+++ b/thirdParty/mavlink/message_definitions/common.xml
@@ -1,892 +1,941 @@
-<?xml version="1.0"?>
+<?xml version='1.0'?>
 <mavlink>
-<version>2</version>
-
-<enums>
-    <enum name="MAV_CMD" >
-      <description>Commands to be executed by the MAV. They can be executed on user request,
+     <version>2</version>
+     <enums>
+          <enum name="MAV_CMD">
+               <description>Commands to be executed by the MAV. They can be executed on user request,
       or as part of a mission script. If the action is used in a mission, the parameter mapping
       to the waypoint/mission message is as follows:
       Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what
       ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data.</description>
-      <entry name="MAV_CMD_NAV_WAYPOINT" value="16">
-        <description>Navigate to waypoint.</description>
-        <param index="1">Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing)</param>
-        <param index="2">Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached)</param>
-        <param index="3">0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.</param>
-        <param index="4">Desired yaw angle at waypoint (rotary wing)</param>
-        <param index="5">Latitude</param>
-        <param index="6">Longitude</param>
-        <param index="7">Altitude</param>
-      </entry>
-      <entry name="MAV_CMD_NAV_LOITER_UNLIM" value="17">
-        <description>Loiter around this waypoint an unlimited amount of time</description>
-        <param index="1">Empty</param>
-        <param index="2">Empty</param>
-        <param index="3">Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise</param>
-        <param index="4">Desired yaw angle.</param>
-        <param index="5">Latitude</param>
-        <param index="6">Longitude</param>
-        <param index="7">Altitude</param>
-      </entry>
-      <entry name="MAV_CMD_NAV_LOITER_TURNS" value="18">
-        <description>Loiter around this waypoint for X turns</description>
-        <param index="1">Turns</param>
-        <param index="2">Empty</param>
-        <param index="3">Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise</param>
-        <param index="4">Desired yaw angle.</param>
-        <param index="5">Latitude</param>
-        <param index="6">Longitude</param>
-        <param index="7">Altitude</param>
-      </entry>
-      <entry name="MAV_CMD_NAV_LOITER_TIME" value="19">
-        <description>Loiter around this waypoint for X seconds</description>
-        <param index="1">Seconds (decimal)</param>
-        <param index="2">Empty</param>
-        <param index="3">Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise</param>
-        <param index="4">Desired yaw angle.</param>
-        <param index="5">Latitude</param>
-        <param index="6">Longitude</param>
-        <param index="7">Altitude</param>
-      </entry>
-      <entry name="MAV_CMD_NAV_RETURN_TO_LAUNCH" value="20">
-        <description>Return to launch location</description>
-        <param index="1">Empty</param>
-        <param index="2">Empty</param>
-        <param index="3">Empty</param>
-        <param index="4">Empty</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_NAV_LAND" value="21">
-      <description>Land at location</description>
-        <param index="1">Empty</param>
-        <param index="2">Empty</param>
-        <param index="3">Empty</param>
-        <param index="4">Desired yaw angle.</param>
-        <param index="5">Latitude</param>
-        <param index="6">Longitude</param>
-        <param index="7">Altitude</param>
-      </entry>
-      <entry name="MAV_CMD_NAV_TAKEOFF" value="22">
-        <description>Takeoff from ground / hand</description>
-        <param index="1">Minimum pitch (if airspeed sensor present), desired pitch without sensor</param>
-        <param index="2">Empty</param>
-        <param index="3">Empty</param>
-        <param index="4">Yaw angle (if magnetometer present), ignored without magnetometer</param>
-        <param index="5">Latitude</param>
-        <param index="6">Longitude</param>
-        <param index="7">Altitude</param>
-        </entry>
-
-      <entry name="MAV_CMD_NAV_ROI" value="80">
-        <description>Sets the region of interest (ROI) for a sensor set or the
+               <entry value="16" name="MAV_CMD_NAV_WAYPOINT">
+                    <description>Navigate to waypoint.</description>
+                    <param index="1">Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing)</param>
+                    <param index="2">Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached)</param>
+                    <param index="3">0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.</param>
+                    <param index="4">Desired yaw angle at waypoint (rotary wing)</param>
+                    <param index="5">Latitude</param>
+                    <param index="6">Longitude</param>
+                    <param index="7">Altitude</param>
+               </entry>
+               <entry value="17" name="MAV_CMD_NAV_LOITER_UNLIM">
+                    <description>Loiter around this waypoint an unlimited amount of time</description>
+                    <param index="1">Empty</param>
+                    <param index="2">Empty</param>
+                    <param index="3">Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise</param>
+                    <param index="4">Desired yaw angle.</param>
+                    <param index="5">Latitude</param>
+                    <param index="6">Longitude</param>
+                    <param index="7">Altitude</param>
+               </entry>
+               <entry value="18" name="MAV_CMD_NAV_LOITER_TURNS">
+                    <description>Loiter around this waypoint for X turns</description>
+                    <param index="1">Turns</param>
+                    <param index="2">Empty</param>
+                    <param index="3">Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise</param>
+                    <param index="4">Desired yaw angle.</param>
+                    <param index="5">Latitude</param>
+                    <param index="6">Longitude</param>
+                    <param index="7">Altitude</param>
+               </entry>
+               <entry value="19" name="MAV_CMD_NAV_LOITER_TIME">
+                    <description>Loiter around this waypoint for X seconds</description>
+                    <param index="1">Seconds (decimal)</param>
+                    <param index="2">Empty</param>
+                    <param index="3">Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise</param>
+                    <param index="4">Desired yaw angle.</param>
+                    <param index="5">Latitude</param>
+                    <param index="6">Longitude</param>
+                    <param index="7">Altitude</param>
+               </entry>
+               <entry value="20" name="MAV_CMD_NAV_RETURN_TO_LAUNCH">
+                    <description>Return to launch location</description>
+                    <param index="1">Empty</param>
+                    <param index="2">Empty</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="21" name="MAV_CMD_NAV_LAND">
+                    <description>Land at location</description>
+                    <param index="1">Empty</param>
+                    <param index="2">Empty</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Desired yaw angle.</param>
+                    <param index="5">Latitude</param>
+                    <param index="6">Longitude</param>
+                    <param index="7">Altitude</param>
+               </entry>
+               <entry value="22" name="MAV_CMD_NAV_TAKEOFF">
+                    <description>Takeoff from ground / hand</description>
+                    <param index="1">Minimum pitch (if airspeed sensor present), desired pitch without sensor</param>
+                    <param index="2">Empty</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Yaw angle (if magnetometer present), ignored without magnetometer</param>
+                    <param index="5">Latitude</param>
+                    <param index="6">Longitude</param>
+                    <param index="7">Altitude</param>
+               </entry>
+               <entry value="80" name="MAV_CMD_NAV_ROI">
+                    <description>Sets the region of interest (ROI) for a sensor set or the
             vehicle itself. This can then be used by the vehicles control
             system to control the vehicle attitude and the attitude of various
             sensors such as cameras.</description>
-        <param index="1">Region of intereset mode. (see MAV_ROI enum)</param>
-        <param index="2">Waypoint index/ target ID. (see MAV_ROI enum)</param>
-        <param index="3">ROI index (allows a vehicle to manage multiple ROI's)</param>
-        <param index="4">Empty</param>
-        <param index="5">x the location of the fixed ROI (see MAV_FRAME)</param>
-        <param index="6">y</param>
-        <param index="7">z</param>
-      </entry>
-
-      <entry name="MAV_CMD_NAV_PATHPLANNING" value="81">
-        <description>Control autonomous path planning on the MAV.</description>
-        <param index="1">0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning</param>
-        <param index="2">0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid</param>
-        <param index="3">Empty</param>
-        <param index="4">Yaw angle at goal, in compass degrees, [0..360]</param>
-        <param index="5">Latitude/X of goal</param>
-        <param index="6">Longitude/Y of goal</param>
-        <param index="7">Altitude/Z of goal</param>
-      </entry>
-      <entry name="MAV_CMD_NAV_LAST" value="95">
-      <description>NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration</description>
-        <param index="1">Empty</param>
-        <param index="2">Empty</param>
-        <param index="3">Empty</param>
-        <param index="4">Empty</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_CONDITION_DELAY" value="112">
-      <description>Delay mission state machine.</description>
-        <param index="1">Delay in seconds (decimal)</param>
-        <param index="2">Empty</param>
-        <param index="3">Empty</param>
-        <param index="4">Empty</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_CONDITION_CHANGE_ALT" value="113">
-        <description>Ascend/descend at rate.  Delay mission state machine until desired altitude reached.</description>
-        <param index="1">Descent / Ascend rate (m/s)</param>
-        <param index="2">Empty</param>
-        <param index="3">Empty</param>
-        <param index="4">Empty</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Finish Altitude</param>
-      </entry>
-      <entry name="MAV_CMD_CONDITION_DISTANCE" value="114">
-        <description>Delay mission state machine until within desired distance of next NAV point.</description>
-        <param index="1">Distance (meters)</param>
-        <param index="2">Empty</param>
-        <param index="3">Empty</param>
-        <param index="4">Empty</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_CONDITION_YAW" value="115">
-        <description>Reach a certain target angle.</description>
-        <param index="1">target angle: [0-360], 0 is north</param>
-        <param index="2">speed during yaw change:[deg per second]</param>
-        <param index="3">direction: negative: counter clockwise, positive: clockwise [-1,1]</param>
-        <param index="4">relative offset or absolute angle: [ 1,0]</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_CONDITION_LAST" value="159">
-      <description>NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration</description>
-        <param index="1">Empty</param>
-        <param index="2">Empty</param>
-        <param index="3">Empty</param>
-        <param index="4">Empty</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_DO_SET_MODE" value="176">
-        <description>Set system mode.</description>
-        <param index="1">Mode, as defined by ENUM MAV_MODE</param>
-        <param index="2">Empty</param>
-        <param index="3">Empty</param>
-        <param index="4">Empty</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_DO_JUMP" value="177">
-        <description>Jump to the desired command in the mission list.  Repeat this action only the specified number of times</description>
-        <param index="1">Sequence number</param>
-        <param index="2">Repeat count</param>
-        <param index="3">Empty</param>
-        <param index="4">Empty</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_DO_CHANGE_SPEED" value="178">
-        <description>Change speed and/or throttle set points.</description>
-        <param index="1">Speed type (0=Airspeed, 1=Ground Speed)</param>
-        <param index="2">Speed  (m/s, -1 indicates no change)</param>
-        <param index="3">Throttle  ( Percent, -1 indicates no change)</param>
-        <param index="4">Empty</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_DO_SET_HOME" value="179">
-        <description>Changes the home location either to the current location or a specified location.</description>
-        <param index="1">Use current (1=use current location, 0=use specified location)</param>
-        <param index="2">Empty</param>
-        <param index="3">Empty</param>
-        <param index="4">Empty</param>
-        <param index="5">Latitude</param>
-        <param index="6">Longitude</param>
-        <param index="7">Altitude</param>
-      </entry>
-      <entry name="MAV_CMD_DO_SET_PARAMETER" value="180">
-        <description>Set a system parameter.  Caution!  Use of this command requires knowledge of the numeric enumeration value of the parameter.</description>
-        <param index="1">Parameter number</param>
-        <param index="2">Parameter value</param>
-        <param index="3">Empty</param>
-        <param index="4">Empty</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_DO_SET_RELAY" value="181">
-        <description>Set a relay to a condition.</description>
-        <param index="1">Relay number</param>
-        <param index="2">Setting (1=on, 0=off, others possible depending on system hardware)</param>
-        <param index="3">Empty</param>
-        <param index="4">Empty</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_DO_REPEAT_RELAY" value="182">
-        <description>Cycle a relay on and off for a desired number of cyles with a desired period.</description>
-        <param index="1">Relay number</param>
-        <param index="2">Cycle count</param>
-        <param index="3">Cycle time (seconds, decimal)</param>
-        <param index="4">Empty</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_DO_SET_SERVO" value="183">
-        <description>Set a servo to a desired PWM value.</description>
-        <param index="1">Servo number</param>
-        <param index="2">PWM (microseconds, 1000 to 2000 typical)</param>
-        <param index="3">Empty</param>
-        <param index="4">Empty</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_DO_REPEAT_SERVO" value="184">
-        <description>Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period.</description>
-        <param index="1">Servo number</param>
-        <param index="2">PWM (microseconds, 1000 to 2000 typical)</param>
-        <param index="3">Cycle count</param>
-        <param index="4">Cycle time (seconds)</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_DO_CONTROL_VIDEO" value="200">
-        <description>Control onboard camera system.</description>
-        <param index="1">Camera ID (-1 for all)</param>
-        <param index="2">Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw</param>
-        <param index="3">Transmission mode: 0: video stream, >0: single images every n seconds (decimal)</param>
-        <param index="4">Recording: 0: disabled, 1: enabled compressed, 2: enabled raw</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_DO_LAST" value="240">
-      <description>NOP - This command is only used to mark the upper limit of the DO commands in the enumeration</description>
-        <param index="1">Empty</param>
-        <param index="2">Empty</param>
-        <param index="3">Empty</param>
-        <param index="4">Empty</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_PREFLIGHT_CALIBRATION" value="241">
-        <description>Trigger calibration. This command will be only accepted if in pre-flight mode.</description>
-        <param index="1">Gyro calibration: 0: no, 1: yes</param>
-        <param index="2">Magnetometer calibration: 0: no, 1: yes</param>
-        <param index="3">Ground pressure: 0: no, 1: yes</param>
-        <param index="4">Radio calibration: 0: no, 1: yes</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-      <entry name="MAV_CMD_PREFLIGHT_STORAGE" value="245">
-        <description>Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode.</description>
-        <param index="1">Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM</param>
-        <param index="2">Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM</param>
-        <param index="3">Reserved</param>
-        <param index="4">Reserved</param>
-        <param index="5">Empty</param>
-        <param index="6">Empty</param>
-        <param index="7">Empty</param>
-      </entry>
-   </enum>
-   <enum name="MAV_DATA_STREAM">
-     <description>Data stream IDs. A data stream is not a fixed set of messages, but rather a
+                    <param index="1">Region of intereset mode. (see MAV_ROI enum)</param>
+                    <param index="2">Waypoint index/ target ID. (see MAV_ROI enum)</param>
+                    <param index="3">ROI index (allows a vehicle to manage multiple ROI's)</param>
+                    <param index="4">Empty</param>
+                    <param index="5">x the location of the fixed ROI (see MAV_FRAME)</param>
+                    <param index="6">y</param>
+                    <param index="7">z</param>
+               </entry>
+               <entry value="81" name="MAV_CMD_NAV_PATHPLANNING">
+                    <description>Control autonomous path planning on the MAV.</description>
+                    <param index="1">0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning</param>
+                    <param index="2">0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Yaw angle at goal, in compass degrees, [0..360]</param>
+                    <param index="5">Latitude/X of goal</param>
+                    <param index="6">Longitude/Y of goal</param>
+                    <param index="7">Altitude/Z of goal</param>
+               </entry>
+               <entry value="95" name="MAV_CMD_NAV_LAST">
+                    <description>NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration</description>
+                    <param index="1">Empty</param>
+                    <param index="2">Empty</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="112" name="MAV_CMD_CONDITION_DELAY">
+                    <description>Delay mission state machine.</description>
+                    <param index="1">Delay in seconds (decimal)</param>
+                    <param index="2">Empty</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="113" name="MAV_CMD_CONDITION_CHANGE_ALT">
+                    <description>Ascend/descend at rate.  Delay mission state machine until desired altitude reached.</description>
+                    <param index="1">Descent / Ascend rate (m/s)</param>
+                    <param index="2">Empty</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Finish Altitude</param>
+               </entry>
+               <entry value="114" name="MAV_CMD_CONDITION_DISTANCE">
+                    <description>Delay mission state machine until within desired distance of next NAV point.</description>
+                    <param index="1">Distance (meters)</param>
+                    <param index="2">Empty</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="115" name="MAV_CMD_CONDITION_YAW">
+                    <description>Reach a certain target angle.</description>
+                    <param index="1">target angle: [0-360], 0 is north</param>
+                    <param index="2">speed during yaw change:[deg per second]</param>
+                    <param index="3">direction: negative: counter clockwise, positive: clockwise [-1,1]</param>
+                    <param index="4">relative offset or absolute angle: [ 1,0]</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="159" name="MAV_CMD_CONDITION_LAST">
+                    <description>NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration</description>
+                    <param index="1">Empty</param>
+                    <param index="2">Empty</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="176" name="MAV_CMD_DO_SET_MODE">
+                    <description>Set system mode.</description>
+                    <param index="1">Mode, as defined by ENUM MAV_MODE</param>
+                    <param index="2">Empty</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="177" name="MAV_CMD_DO_JUMP">
+                    <description>Jump to the desired command in the mission list.  Repeat this action only the specified number of times</description>
+                    <param index="1">Sequence number</param>
+                    <param index="2">Repeat count</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="178" name="MAV_CMD_DO_CHANGE_SPEED">
+                    <description>Change speed and/or throttle set points.</description>
+                    <param index="1">Speed type (0=Airspeed, 1=Ground Speed)</param>
+                    <param index="2">Speed  (m/s, -1 indicates no change)</param>
+                    <param index="3">Throttle  ( Percent, -1 indicates no change)</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="179" name="MAV_CMD_DO_SET_HOME">
+                    <description>Changes the home location either to the current location or a specified location.</description>
+                    <param index="1">Use current (1=use current location, 0=use specified location)</param>
+                    <param index="2">Empty</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Latitude</param>
+                    <param index="6">Longitude</param>
+                    <param index="7">Altitude</param>
+               </entry>
+               <entry value="180" name="MAV_CMD_DO_SET_PARAMETER">
+                    <description>Set a system parameter.  Caution!  Use of this command requires knowledge of the numeric enumeration value of the parameter.</description>
+                    <param index="1">Parameter number</param>
+                    <param index="2">Parameter value</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="181" name="MAV_CMD_DO_SET_RELAY">
+                    <description>Set a relay to a condition.</description>
+                    <param index="1">Relay number</param>
+                    <param index="2">Setting (1=on, 0=off, others possible depending on system hardware)</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="182" name="MAV_CMD_DO_REPEAT_RELAY">
+                    <description>Cycle a relay on and off for a desired number of cyles with a desired period.</description>
+                    <param index="1">Relay number</param>
+                    <param index="2">Cycle count</param>
+                    <param index="3">Cycle time (seconds, decimal)</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="183" name="MAV_CMD_DO_SET_SERVO">
+                    <description>Set a servo to a desired PWM value.</description>
+                    <param index="1">Servo number</param>
+                    <param index="2">PWM (microseconds, 1000 to 2000 typical)</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="184" name="MAV_CMD_DO_REPEAT_SERVO">
+                    <description>Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period.</description>
+                    <param index="1">Servo number</param>
+                    <param index="2">PWM (microseconds, 1000 to 2000 typical)</param>
+                    <param index="3">Cycle count</param>
+                    <param index="4">Cycle time (seconds)</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="200" name="MAV_CMD_DO_CONTROL_VIDEO">
+                    <description>Control onboard camera capturing.</description>
+                    <param index="1">Camera ID (-1 for all)</param>
+                    <param index="2">Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw</param>
+                    <param index="3">Transmission mode: 0: video stream, >0: single images every n seconds (decimal)</param>
+                    <param index="4">Recording: 0: disabled, 1: enabled compressed, 2: enabled raw</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="201" name="MAV_CMD_DO_SET_ROI">
+                    <description>Sets the region of interest (ROI) for a sensor set or the
+                    vehicle itself. This can then be used by the vehicles control
+                    system to control the vehicle attitude and the attitude of various
+                    devices such as cameras.
+                </description>
+                    <param index="1">Region of interest mode. (see MAV_ROI enum)</param>
+                    <param index="2">Waypoint index/ target ID. (see MAV_ROI enum)</param>
+                    <param index="3">ROI index (allows a vehicle to manage multiple cameras etc.)</param>
+                    <param index="4">Empty</param>
+                    <param index="5">x the location of the fixed ROI (see MAV_FRAME)</param>
+                    <param index="6">y</param>
+                    <param index="7">z</param>
+               </entry>
+               <entry value="240" name="MAV_CMD_DO_LAST">
+                    <description>NOP - This command is only used to mark the upper limit of the DO commands in the enumeration</description>
+                    <param index="1">Empty</param>
+                    <param index="2">Empty</param>
+                    <param index="3">Empty</param>
+                    <param index="4">Empty</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="241" name="MAV_CMD_PREFLIGHT_CALIBRATION">
+                    <description>Trigger calibration. This command will be only accepted if in pre-flight mode.</description>
+                    <param index="1">Gyro calibration: 0: no, 1: yes</param>
+                    <param index="2">Magnetometer calibration: 0: no, 1: yes</param>
+                    <param index="3">Ground pressure: 0: no, 1: yes</param>
+                    <param index="4">Radio calibration: 0: no, 1: yes</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+               <entry value="245" name="MAV_CMD_PREFLIGHT_STORAGE">
+                    <description>Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode.</description>
+                    <param index="1">Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM</param>
+                    <param index="2">Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM</param>
+                    <param index="3">Reserved</param>
+                    <param index="4">Reserved</param>
+                    <param index="5">Empty</param>
+                    <param index="6">Empty</param>
+                    <param index="7">Empty</param>
+               </entry>
+          </enum>
+          <enum name="MAV_DATA_STREAM">
+               <description>Data stream IDs. A data stream is not a fixed set of messages, but rather a
      recommendation to the autopilot software. Individual autopilots may or may not obey
      the recommended messages.
      </description>
-     <entry name="MAV_DATA_STREAM_ALL" value="0"><description>Enable all data streams</description></entry>
-     <entry name="MAV_DATA_STREAM_RAW_SENSORS" value="1"><description>Enable IMU_RAW, GPS_RAW, GPS_STATUS packets.</description></entry>
-     <entry name="MAV_DATA_STREAM_EXTENDED_STATUS" value="2"><description>Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS</description></entry>
-     <entry name="MAV_DATA_STREAM_RC_CHANNELS" value="3"><description>Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW</description></entry>
-     <entry name="MAV_DATA_STREAM_RAW_CONTROLLER" value="4"><description>Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT.</description></entry>
-     <entry name="MAV_DATA_STREAM_POSITION" value="6"><description>Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages.</description></entry>
-     <entry name="MAV_DATA_STREAM_EXTRA1" value="10"><description>Dependent on the autopilot</description></entry>
-     <entry name="MAV_DATA_STREAM_EXTRA2" value="11"><description>Dependent on the autopilot</description></entry>
-     <entry name="MAV_DATA_STREAM_EXTRA3" value="12"><description>Dependent on the autopilot</description></entry>
-   </enum>
-
-   <enum name="MAV_ROI">
-     <description> The ROI (region of interest) for the vehicle. This can be
-      be used by the vehicle for camera/vehicle attitude alignment (see
-	  MAV_CMD_NAV_ROI).
-     </description>
-     <entry name="MAV_ROI_WPNEXT" value="0"><description>Point toward next waypoint.</description></entry>
-     <entry name="MAV_ROI_WPINDEX" value="1"><description>Point toward given waypoint.</description></entry>
-     <entry name="MAV_ROI_LOCATION" value="2"><description>Point toward fixed location.</description></entry>
-     <entry name="MAV_ROI_TARGET" value="3"><description>Point toward of given id.</description></entry>
-   </enum>
-
-</enums>
-
-<messages>
-   <message name="HEARTBEAT" id="0">
-     <description>The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot).</description>
-     <field name="type" type="uint8_t">Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)</field>
-     <field name="autopilot" type="uint8_t">Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM</field>
-     <field name="mavlink_version" type="uint8_t_mavlink_version">MAVLink version</field>
-   </message>
- 
-   <message name="BOOT" id="1">
-	<description>The boot message indicates that a system is starting. The onboard software version allows to keep track of onboard soft/firmware revisions.</description>
-     <field name="version" type="uint32_t">The onboard software version</field>
-   </message>
-
-   <message name="SYSTEM_TIME" id="2">
-     <description>The system time is the time of the master clock, typically the computer clock of the main onboard computer.</description>
-     <field name="time_usec" type="uint64_t">Timestamp of the master clock in microseconds since UNIX epoch.</field>
-   </message>
-
-   <message name="PING" id="3">
-     <description>A ping message either requesting or responding to a ping. This allows to measure the system latencies, including serial port, radio modem and UDP connections.</description>
-     <field name="seq" type="uint32_t">PING sequence</field>
-     <field name="target_system" type="uint8_t">0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system</field>
-     <field name="target_component" type="uint8_t">0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system</field>
-     <field name="time" type="uint64_t">Unix timestamp in microseconds</field>
-   </message>
-   
-  <message name="SYSTEM_TIME_UTC" id="4">
-    <description>UTC date and time from GPS module</description>
-    <field name="utc_date" type="uint32_t">GPS UTC date ddmmyy</field>
-    <field name="utc_time" type="uint32_t">GPS UTC time hhmmss</field>
-  </message>
-  
-  <message name="CHANGE_OPERATOR_CONTROL" id="5">
-    <description>Request to control this MAV</description>
-    <field name="target_system" type="uint8_t">System the GCS requests control for</field>
-    <field name="control_request" type="uint8_t">0: request control of this MAV, 1: Release control of this MAV</field>
-    <field name="version" type="uint8_t">0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch.</field>
-    <field name="passkey" type="char[25]">Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-"</field>
-  </message>
-  
-  <message name="CHANGE_OPERATOR_CONTROL_ACK" id="6">
-    <description>Accept / deny control of this MAV</description>
-    <field name="gcs_system_id" type="uint8_t">ID of the GCS this message </field>
-    <field name="control_request" type="uint8_t">0: request control of this MAV, 1: Release control of this MAV</field>
-    <field name="ack" type="uint8_t">0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control</field>
-  </message>
-  
-  <message name="AUTH_KEY" id="7">
-    <description>Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety.</description>
-    <field name="key" type="char[32]">key</field>
-  </message>
-  
-   <message name="ACTION_ACK" id="9">
-	<description>This message acknowledges an action. IMPORTANT: The acknowledgement can be also negative, e.g. the MAV rejects a reset message because it is in-flight. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h</description>
-     <field name="action" type="uint8_t">The action id</field>
-     <field name="result" type="uint8_t">0: Action DENIED, 1: Action executed</field>
-   </message>
-
-   <message name="ACTION" id="10">
-	<description>An action message allows to execute a certain onboard action. These include liftoff, land, storing parameters too EEPROM, shutddown, etc. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h</description>
-     <field name="target" type="uint8_t">The system executing the action</field>
-     <field name="target_component" type="uint8_t">The component executing the action</field>
-     <field name="action" type="uint8_t">The action id</field>
-   </message>
-
-   <message name="SET_MODE" id="11">
-   <description>Set the system mode, as defined by enum MAV_MODE in mavlink/include/mavlink_types.h. There is no target component id as the mode is by definition for the overall aircraft, not only for one component.</description>
-     <field name="target" type="uint8_t">The system setting the mode</field>
-     <field name="mode" type="uint8_t">The new mode</field>
-   </message>
-
-   <message name="SET_NAV_MODE" id="12">
-   <description>Set the system navigation mode, as defined by enum MAV_NAV_MODE in mavlink/include/mavlink_types.h. The navigation mode applies to the whole aircraft and thus all components.</description>
-     <field name="target" type="uint8_t">The system setting the mode</field>
-     <field name="nav_mode" type="uint8_t">The new navigation mode</field>
-   </message>
-   
-   <message name="PARAM_REQUEST_READ" id="20">
-     <description>Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code.</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-     <field name="param_id" type="array[15]">Onboard parameter id</field>
-     <field name="param_index" type="int16_t">Parameter index. Send -1 to use the param ID field as identifier</field>
-   </message>
-
-   <message name="PARAM_REQUEST_LIST" id="21">
-     <description>Request all parameters of this component. After his request, all parameters are emitted.</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-   </message>
-
-   <message name="PARAM_VALUE" id="22">
-     <description>Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout.</description>
-     <field name="param_id" type="array[15]">Onboard parameter id</field>
-     <field name="param_value" type="float">Onboard parameter value</field>
-     <field name="param_count" type="uint16_t">Total number of onboard parameters</field>
-     <field name="param_index" type="uint16_t">Index of this onboard parameter</field>
-   </message>
-
-   <message name="PARAM_SET" id="23">
-     <description>Set a parameter value TEMPORARILY to RAM. It will be reset to default on system reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter value by sending a param_value message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message.</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-     <field name="param_id" type="array[15]">Onboard parameter id</field>
-     <field name="param_value" type="float">Onboard parameter value</field>
-   </message>
-   
-         <message name="GPS_RAW_INT" id="25">
-     <description>The global position, as returned by the Global Positioning System (GPS). This is
+               <entry value="0" name="MAV_DATA_STREAM_ALL">
+                    <description>Enable all data streams</description>
+               </entry>
+               <entry value="1" name="MAV_DATA_STREAM_RAW_SENSORS">
+                    <description>Enable IMU_RAW, GPS_RAW, GPS_STATUS packets.</description>
+               </entry>
+               <entry value="2" name="MAV_DATA_STREAM_EXTENDED_STATUS">
+                    <description>Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS</description>
+               </entry>
+               <entry value="3" name="MAV_DATA_STREAM_RC_CHANNELS">
+                    <description>Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW</description>
+               </entry>
+               <entry value="4" name="MAV_DATA_STREAM_RAW_CONTROLLER">
+                    <description>Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT.</description>
+               </entry>
+               <entry value="6" name="MAV_DATA_STREAM_POSITION">
+                    <description>Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages.</description>
+               </entry>
+               <entry value="10" name="MAV_DATA_STREAM_EXTRA1">
+                    <description>Dependent on the autopilot</description>
+               </entry>
+               <entry value="11" name="MAV_DATA_STREAM_EXTRA2">
+                    <description>Dependent on the autopilot</description>
+               </entry>
+               <entry value="12" name="MAV_DATA_STREAM_EXTRA3">
+                    <description>Dependent on the autopilot</description>
+               </entry>
+          </enum>
+          <enum name="MAV_ROI">
+               <description> The ROI (region of interest) for the vehicle. This can be
+                be used by the vehicle for camera/vehicle attitude alignment (see
+                MAV_CMD_NAV_ROI).
+            </description>
+               <entry value="0" name="MAV_ROI_NONE">
+                    <description>No region of interest.</description>
+               </entry>
+               <entry value="1" name="MAV_ROI_WPNEXT">
+                    <description>Point toward next waypoint.</description>
+               </entry>
+               <entry value="2" name="MAV_ROI_WPINDEX">
+                    <description>Point toward given waypoint.</description>
+               </entry>
+               <entry value="3" name="MAV_ROI_LOCATION">
+                    <description>Point toward fixed location.</description>
+               </entry>
+               <entry value="4" name="MAV_ROI_TARGET">
+                    <description>Point toward of given id.</description>
+               </entry>
+          </enum>
+     </enums>
+     <messages>
+          <message id="0" name="HEARTBEAT">
+               <description>The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot).</description>
+               <field type="uint8_t" name="type">Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)</field>
+               <field type="uint8_t" name="autopilot">Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM</field>
+               <field type="uint8_t_mavlink_version" name="mavlink_version">MAVLink version</field>
+          </message>
+          <message id="1" name="BOOT">
+               <description>The boot message indicates that a system is starting. The onboard software version allows to keep track of onboard soft/firmware revisions.</description>
+               <field type="uint32_t" name="version">The onboard software version</field>
+          </message>
+          <message id="2" name="SYSTEM_TIME">
+               <description>The system time is the time of the master clock, typically the computer clock of the main onboard computer.</description>
+               <field type="uint64_t" name="time_usec">Timestamp of the master clock in microseconds since UNIX epoch.</field>
+          </message>
+          <message id="3" name="PING">
+               <description>A ping message either requesting or responding to a ping. This allows to measure the system latencies, including serial port, radio modem and UDP connections.</description>
+               <field type="uint32_t" name="seq">PING sequence</field>
+               <field type="uint8_t" name="target_system">0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system</field>
+               <field type="uint8_t" name="target_component">0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system</field>
+               <field type="uint64_t" name="time">Unix timestamp in microseconds</field>
+          </message>
+          <message id="4" name="SYSTEM_TIME_UTC">
+               <description>UTC date and time from GPS module</description>
+               <field type="uint32_t" name="utc_date">GPS UTC date ddmmyy</field>
+               <field type="uint32_t" name="utc_time">GPS UTC time hhmmss</field>
+          </message>
+          <message id="5" name="CHANGE_OPERATOR_CONTROL">
+               <description>Request to control this MAV</description>
+               <field type="uint8_t" name="target_system">System the GCS requests control for</field>
+               <field type="uint8_t" name="control_request">0: request control of this MAV, 1: Release control of this MAV</field>
+               <field type="uint8_t" name="version">0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch.</field>
+               <field type="char[25]" name="passkey">Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-"</field>
+          </message>
+          <message id="6" name="CHANGE_OPERATOR_CONTROL_ACK">
+               <description>Accept / deny control of this MAV</description>
+               <field type="uint8_t" name="gcs_system_id">ID of the GCS this message </field>
+               <field type="uint8_t" name="control_request">0: request control of this MAV, 1: Release control of this MAV</field>
+               <field type="uint8_t" name="ack">0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control</field>
+          </message>
+          <message id="7" name="AUTH_KEY">
+               <description>Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety.</description>
+               <field type="char[32]" name="key">key</field>
+          </message>
+          <message id="9" name="ACTION_ACK">
+               <description>This message acknowledges an action. IMPORTANT: The acknowledgement can be also negative, e.g. the MAV rejects a reset message because it is in-flight. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h</description>
+               <field type="uint8_t" name="action">The action id</field>
+               <field type="uint8_t" name="result">0: Action DENIED, 1: Action executed</field>
+          </message>
+          <message id="10" name="ACTION">
+               <description>An action message allows to execute a certain onboard action. These include liftoff, land, storing parameters too EEPROM, shutddown, etc. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h</description>
+               <field type="uint8_t" name="target">The system executing the action</field>
+               <field type="uint8_t" name="target_component">The component executing the action</field>
+               <field type="uint8_t" name="action">The action id</field>
+          </message>
+          <message id="11" name="SET_MODE">
+               <description>Set the system mode, as defined by enum MAV_MODE in mavlink/include/mavlink_types.h. There is no target component id as the mode is by definition for the overall aircraft, not only for one component.</description>
+               <field type="uint8_t" name="target">The system setting the mode</field>
+               <field type="uint8_t" name="mode">The new mode</field>
+          </message>
+          <message id="12" name="SET_NAV_MODE">
+               <description>Set the system navigation mode, as defined by enum MAV_NAV_MODE in mavlink/include/mavlink_types.h. The navigation mode applies to the whole aircraft and thus all components.</description>
+               <field type="uint8_t" name="target">The system setting the mode</field>
+               <field type="uint8_t" name="nav_mode">The new navigation mode</field>
+          </message>
+          <message id="20" name="PARAM_REQUEST_READ">
+               <description>Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code.</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+               <field type="array[15]" name="param_id">Onboard parameter id</field>
+               <field type="int16_t" name="param_index">Parameter index. Send -1 to use the param ID field as identifier</field>
+          </message>
+          <message id="21" name="PARAM_REQUEST_LIST">
+               <description>Request all parameters of this component. After his request, all parameters are emitted.</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+          </message>
+          <message id="22" name="PARAM_VALUE">
+               <description>Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout.</description>
+               <field type="array[15]" name="param_id">Onboard parameter id</field>
+               <field type="float" name="param_value">Onboard parameter value</field>
+               <field type="uint16_t" name="param_count">Total number of onboard parameters</field>
+               <field type="uint16_t" name="param_index">Index of this onboard parameter</field>
+          </message>
+          <message id="23" name="PARAM_SET">
+               <description>Set a parameter value TEMPORARILY to RAM. It will be reset to default on system reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter value by sending a param_value message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message.</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+               <field type="array[15]" name="param_id">Onboard parameter id</field>
+               <field type="float" name="param_value">Onboard parameter value</field>
+          </message>
+          <message id="25" name="GPS_RAW_INT">
+               <description>The global position, as returned by the Global Positioning System (GPS). This is
 NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame)</description>
-     <field name="usec" type="uint64_t">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
-     <field name="fix_type" type="uint8_t">0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.</field>
-     <field name="lat"   type="int32_t">Latitude in 1E7 degrees</field>
-     <field name="lon"   type="int32_t">Longitude in 1E7 degrees</field>
-     <field name="alt"   type="int32_t">Altitude in 1E3 meters (millimeters)</field>
-     <field name="eph"  type="float">GPS HDOP</field>
-     <field name="epv"  type="float">GPS VDOP</field>
-     <field name="v"  type="float">GPS ground speed (m/s)</field>
-     <field name="hdg"  type="float">Compass heading in degrees, 0..360 degrees</field>
-   </message>
-   
-   <message name="SCALED_IMU" id="26">
-     <description>The RAW IMU readings for the usual 9DOF sensor setup. This message should contain the scaled values to the described units</description>
-     <field name="usec"  type="uint64_t">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
-     <field name="xacc"  type="int16_t">X acceleration (mg)</field>
-     <field name="yacc"  type="int16_t">Y acceleration (mg)</field>
-     <field name="zacc"  type="int16_t">Z acceleration (mg)</field>
-     <field name="xgyro" type="int16_t">Angular speed around X axis (millirad /sec)</field>
-     <field name="ygyro" type="int16_t">Angular speed around Y axis (millirad /sec)</field>
-     <field name="zgyro" type="int16_t">Angular speed around Z axis (millirad /sec)</field>
-     <field name="xmag"  type="int16_t">X Magnetic field (milli tesla)</field>
-     <field name="ymag"  type="int16_t">Y Magnetic field (milli tesla)</field>
-     <field name="zmag"  type="int16_t">Z Magnetic field (milli tesla)</field>
-   </message>
-   
-   <message name="GPS_STATUS" id="27">
-     <description>The positioning status, as reported by GPS. This message is intended to display status information about each satellite visible to the receiver. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites.</description>
-     <field name="satellites_visible"   type="uint8_t">Number of satellites visible</field>
-     <field name="satellite_prn"   type="array[20]">Global satellite ID</field>
-     <field name="satellite_used"   type="array[20]">0: Satellite not used, 1: used for localization</field>
-     <field name="satellite_elevation"   type="array[20]">Elevation (0: right on top of receiver, 90: on the horizon) of satellite</field>
-     <field name="satellite_azimuth"  type="array[20]">Direction of satellite, 0: 0 deg, 255: 360 deg.</field>
-     <field name="satellite_snr"  type="array[20]">Signal to noise ratio of satellite</field>
-   </message>
-
-   <message name="RAW_IMU" id="28">
-     <description>The RAW IMU readings for the usual 9DOF sensor setup. This message should always contain the true raw values without any scaling to allow data capture and system debugging.</description>
-     <field name="usec"  type="uint64_t">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
-     <field name="xacc"  type="int16_t">X acceleration (raw)</field>
-     <field name="yacc"  type="int16_t">Y acceleration (raw)</field>
-     <field name="zacc"  type="int16_t">Z acceleration (raw)</field>
-     <field name="xgyro" type="int16_t">Angular speed around X axis (raw)</field>
-     <field name="ygyro" type="int16_t">Angular speed around Y axis (raw)</field>
-     <field name="zgyro" type="int16_t">Angular speed around Z axis (raw)</field>
-     <field name="xmag"  type="int16_t">X Magnetic field (raw)</field>
-     <field name="ymag"  type="int16_t">Y Magnetic field (raw)</field>
-     <field name="zmag"  type="int16_t">Z Magnetic field (raw)</field>
-   </message>
-
-  <message name="RAW_PRESSURE" id="29">
-     <description>The RAW pressure readings for the typical setup of one absolute pressure and one differential pressure sensor. The sensor values should be the raw, UNSCALED ADC values.</description>
-     <field name="usec"  type="uint64_t">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
-     <field name="press_abs"    type="int16_t">Absolute pressure (raw)</field>
-     <field name="press_diff1"  type="int16_t">Differential pressure 1 (raw)</field>
-     <field name="press_diff2"  type="int16_t">Differential pressure 2 (raw)</field>
-     <field name="temperature"  type="int16_t">Raw Temperature measurement (raw)</field>
-   </message>
-
-  <message name="SCALED_PRESSURE" id="38">
-     <description>The pressure readings for the typical setup of one absolute and differential pressure sensor. The units are as specified in each field.</description>
-     <field name="usec"  type="uint64_t">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
-     <field name="press_abs"  type="float">Absolute pressure (hectopascal)</field>
-     <field name="press_diff"  type="float">Differential pressure 1 (hectopascal)</field>
-     <field name="temperature"  type="int16_t">Temperature measurement (0.01 degrees celsius)</field>
-   </message>
-
-
-   <message name="ATTITUDE" id="30">
-    <description>The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right).</description>
-     <field name="usec"  type="uint64_t">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
-     <field name="roll"  type="float">Roll angle (rad)</field>
-     <field name="pitch" type="float">Pitch angle (rad)</field>
-     <field name="yaw"   type="float">Yaw angle (rad)</field>
-     <field name="rollspeed"  type="float">Roll angular speed (rad/s)</field>
-     <field name="pitchspeed" type="float">Pitch angular speed (rad/s)</field>
-     <field name="yawspeed"   type="float">Yaw angular speed (rad/s)</field>
-   </message>
-
-   <message name="LOCAL_POSITION" id="31">
-     <description>The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame)</description>
-     <field name="usec" type="uint64_t">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
-     <field name="x"   type="float">X Position</field>
-     <field name="y"   type="float">Y Position</field>
-     <field name="z"   type="float">Z Position</field>
-     <field name="vx"  type="float">X Speed</field>
-     <field name="vy"  type="float">Y Speed</field>
-     <field name="vz"  type="float">Z Speed</field>
-   </message>
-   
-
-   <message name="GLOBAL_POSITION" id="33">
-     <description>The filtered global position (e.g. fused GPS and accelerometers). Coordinate frame is right-handed, Z-axis up (GPS frame)</description>
-     <field name="usec" type="uint64_t">Timestamp (microseconds since unix epoch)</field>
-     <field name="lat"   type="float">Latitude, in degrees</field>
-     <field name="lon"   type="float">Longitude, in degrees</field>
-     <field name="alt"   type="float">Absolute altitude, in meters</field>
-     <field name="vx"  type="float">X Speed (in Latitude direction, positive: going north)</field>
-     <field name="vy"  type="float">Y Speed (in Longitude direction, positive: going east)</field>
-     <field name="vz"  type="float">Z Speed (in Altitude direction, positive: going up)</field>
-   </message>
-
-   <message name="GPS_RAW" id="32">
-     <description>The global position, as returned by the Global Positioning System (GPS). This is
+               <field type="uint64_t" name="usec">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
+               <field type="uint8_t" name="fix_type">0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.</field>
+               <field type="int32_t" name="lat">Latitude in 1E7 degrees</field>
+               <field type="int32_t" name="lon">Longitude in 1E7 degrees</field>
+               <field type="int32_t" name="alt">Altitude in 1E3 meters (millimeters)</field>
+               <field type="float" name="eph">GPS HDOP</field>
+               <field type="float" name="epv">GPS VDOP</field>
+               <field type="float" name="v">GPS ground speed (m/s)</field>
+               <field type="float" name="hdg">Compass heading in degrees, 0..360 degrees</field>
+          </message>
+          <message id="26" name="SCALED_IMU">
+               <description>The RAW IMU readings for the usual 9DOF sensor setup. This message should contain the scaled values to the described units</description>
+               <field type="uint64_t" name="usec">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
+               <field type="int16_t" name="xacc">X acceleration (mg)</field>
+               <field type="int16_t" name="yacc">Y acceleration (mg)</field>
+               <field type="int16_t" name="zacc">Z acceleration (mg)</field>
+               <field type="int16_t" name="xgyro">Angular speed around X axis (millirad /sec)</field>
+               <field type="int16_t" name="ygyro">Angular speed around Y axis (millirad /sec)</field>
+               <field type="int16_t" name="zgyro">Angular speed around Z axis (millirad /sec)</field>
+               <field type="int16_t" name="xmag">X Magnetic field (milli tesla)</field>
+               <field type="int16_t" name="ymag">Y Magnetic field (milli tesla)</field>
+               <field type="int16_t" name="zmag">Z Magnetic field (milli tesla)</field>
+          </message>
+          <message id="27" name="GPS_STATUS">
+               <description>The positioning status, as reported by GPS. This message is intended to display status information about each satellite visible to the receiver. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites.</description>
+               <field type="uint8_t" name="satellites_visible">Number of satellites visible</field>
+               <field type="array[20]" name="satellite_prn">Global satellite ID</field>
+               <field type="array[20]" name="satellite_used">0: Satellite not used, 1: used for localization</field>
+               <field type="array[20]" name="satellite_elevation">Elevation (0: right on top of receiver, 90: on the horizon) of satellite</field>
+               <field type="array[20]" name="satellite_azimuth">Direction of satellite, 0: 0 deg, 255: 360 deg.</field>
+               <field type="array[20]" name="satellite_snr">Signal to noise ratio of satellite</field>
+          </message>
+          <message id="28" name="RAW_IMU">
+               <description>The RAW IMU readings for the usual 9DOF sensor setup. This message should always contain the true raw values without any scaling to allow data capture and system debugging.</description>
+               <field type="uint64_t" name="usec">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
+               <field type="int16_t" name="xacc">X acceleration (raw)</field>
+               <field type="int16_t" name="yacc">Y acceleration (raw)</field>
+               <field type="int16_t" name="zacc">Z acceleration (raw)</field>
+               <field type="int16_t" name="xgyro">Angular speed around X axis (raw)</field>
+               <field type="int16_t" name="ygyro">Angular speed around Y axis (raw)</field>
+               <field type="int16_t" name="zgyro">Angular speed around Z axis (raw)</field>
+               <field type="int16_t" name="xmag">X Magnetic field (raw)</field>
+               <field type="int16_t" name="ymag">Y Magnetic field (raw)</field>
+               <field type="int16_t" name="zmag">Z Magnetic field (raw)</field>
+          </message>
+          <message id="29" name="RAW_PRESSURE">
+               <description>The RAW pressure readings for the typical setup of one absolute pressure and one differential pressure sensor. The sensor values should be the raw, UNSCALED ADC values.</description>
+               <field type="uint64_t" name="usec">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
+               <field type="int16_t" name="press_abs">Absolute pressure (raw)</field>
+               <field type="int16_t" name="press_diff1">Differential pressure 1 (raw)</field>
+               <field type="int16_t" name="press_diff2">Differential pressure 2 (raw)</field>
+               <field type="int16_t" name="temperature">Raw Temperature measurement (raw)</field>
+          </message>
+          <message id="38" name="SCALED_PRESSURE">
+               <description>The pressure readings for the typical setup of one absolute and differential pressure sensor. The units are as specified in each field.</description>
+               <field type="uint64_t" name="usec">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
+               <field type="float" name="press_abs">Absolute pressure (hectopascal)</field>
+               <field type="float" name="press_diff">Differential pressure 1 (hectopascal)</field>
+               <field type="int16_t" name="temperature">Temperature measurement (0.01 degrees celsius)</field>
+          </message>
+          <message id="30" name="ATTITUDE">
+               <description>The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right).</description>
+               <field type="uint64_t" name="usec">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
+               <field type="float" name="roll">Roll angle (rad)</field>
+               <field type="float" name="pitch">Pitch angle (rad)</field>
+               <field type="float" name="yaw">Yaw angle (rad)</field>
+               <field type="float" name="rollspeed">Roll angular speed (rad/s)</field>
+               <field type="float" name="pitchspeed">Pitch angular speed (rad/s)</field>
+               <field type="float" name="yawspeed">Yaw angular speed (rad/s)</field>
+          </message>
+          <message id="31" name="LOCAL_POSITION">
+               <description>The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame)</description>
+               <field type="uint64_t" name="usec">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
+               <field type="float" name="x">X Position</field>
+               <field type="float" name="y">Y Position</field>
+               <field type="float" name="z">Z Position</field>
+               <field type="float" name="vx">X Speed</field>
+               <field type="float" name="vy">Y Speed</field>
+               <field type="float" name="vz">Z Speed</field>
+          </message>
+          <message id="33" name="GLOBAL_POSITION">
+               <description>The filtered global position (e.g. fused GPS and accelerometers). Coordinate frame is right-handed, Z-axis up (GPS frame)</description>
+               <field type="uint64_t" name="usec">Timestamp (microseconds since unix epoch)</field>
+               <field type="float" name="lat">Latitude, in degrees</field>
+               <field type="float" name="lon">Longitude, in degrees</field>
+               <field type="float" name="alt">Absolute altitude, in meters</field>
+               <field type="float" name="vx">X Speed (in Latitude direction, positive: going north)</field>
+               <field type="float" name="vy">Y Speed (in Longitude direction, positive: going east)</field>
+               <field type="float" name="vz">Z Speed (in Altitude direction, positive: going up)</field>
+          </message>
+          <message id="32" name="GPS_RAW">
+               <description>The global position, as returned by the Global Positioning System (GPS). This is
 NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame)</description>
-     <field name="usec" type="uint64_t">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
-     <field name="fix_type" type="uint8_t">0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.</field>
-     <field name="lat"   type="float">Latitude in degrees</field>
-     <field name="lon"   type="float">Longitude in degrees</field>
-     <field name="alt"   type="float">Altitude in meters</field>
-     <field name="eph"  type="float">GPS HDOP</field>
-     <field name="epv"  type="float">GPS VDOP</field>
-     <field name="v"  type="float">GPS ground speed</field>
-     <field name="hdg"  type="float">Compass heading in degrees, 0..360 degrees</field>
-   </message>
-
-   <message name="SYS_STATUS" id="34">
-     <description>The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows wether the system is currently active or not and if an emergency occured. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occured it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout.</description>
-     <field name="mode" type="uint8_t">System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h</field>
-     <field name="nav_mode" type="uint8_t">Navigation mode, see MAV_NAV_MODE ENUM</field>
-     <field name="status" type="uint8_t">System status flag, see MAV_STATUS ENUM</field>
-     <field name="load" type="uint16_t">Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000</field>
-     <field name="vbat" type="uint16_t">Battery voltage, in millivolts (1 = 1 millivolt)</field>
-     <field name="battery_remaining" type="uint16_t">Remaining battery energy: (0%: 0, 100%: 1000)</field>
-     <field name="packet_drop" type="uint16_t">Dropped packets (packets that were corrupted on reception on the MAV)</field>
-   </message>
-
-   <message name="RC_CHANNELS_RAW" id="35">
-     <description>The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification.</description>
-     <field name="chan1_raw" type="uint16_t">RC channel 1 value, in microseconds</field>
-     <field name="chan2_raw" type="uint16_t">RC channel 2 value, in microseconds</field>
-     <field name="chan3_raw" type="uint16_t">RC channel 3 value, in microseconds</field>
-     <field name="chan4_raw" type="uint16_t">RC channel 4 value, in microseconds</field>
-     <field name="chan5_raw" type="uint16_t">RC channel 5 value, in microseconds</field>
-     <field name="chan6_raw" type="uint16_t">RC channel 6 value, in microseconds</field>
-     <field name="chan7_raw" type="uint16_t">RC channel 7 value, in microseconds</field>
-     <field name="chan8_raw" type="uint16_t">RC channel 8 value, in microseconds</field>
-     <field name="rssi" type="uint8_t">Receive signal strength indicator, 0: 0%, 255: 100%</field>
-   </message>
-
-   <message name="RC_CHANNELS_SCALED" id="36">
-     <description>The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000</description>
-     <field name="chan1_scaled" type="int16_t">RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
-     <field name="chan2_scaled" type="int16_t">RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
-     <field name="chan3_scaled" type="int16_t">RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
-     <field name="chan4_scaled" type="int16_t">RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
-     <field name="chan5_scaled" type="int16_t">RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
-     <field name="chan6_scaled" type="int16_t">RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
-     <field name="chan7_scaled" type="int16_t">RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
-     <field name="chan8_scaled" type="int16_t">RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
-     <field name="rssi" type="uint8_t">Receive signal strength indicator, 0: 0%, 255: 100%</field>
-   </message>
-   
-     <message name="SERVO_OUTPUT_RAW" id="37">
-    <description>The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%.</description>
-    <field name="servo1_raw" type="uint16_t">Servo output 1 value, in microseconds</field>
-    <field name="servo2_raw" type="uint16_t">Servo output 2 value, in microseconds</field>
-    <field name="servo3_raw" type="uint16_t">Servo output 3 value, in microseconds</field>
-    <field name="servo4_raw" type="uint16_t">Servo output 4 value, in microseconds</field>
-    <field name="servo5_raw" type="uint16_t">Servo output 5 value, in microseconds</field>
-    <field name="servo6_raw" type="uint16_t">Servo output 6 value, in microseconds</field>
-    <field name="servo7_raw" type="uint16_t">Servo output 7 value, in microseconds</field>
-    <field name="servo8_raw" type="uint16_t">Servo output 8 value, in microseconds</field>
-  </message> 
-
-   <message name="WAYPOINT" id="39">
-     <description>Message encoding a waypoint. This message is emitted to announce
+               <field type="uint64_t" name="usec">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
+               <field type="uint8_t" name="fix_type">0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.</field>
+               <field type="float" name="lat">Latitude in degrees</field>
+               <field type="float" name="lon">Longitude in degrees</field>
+               <field type="float" name="alt">Altitude in meters</field>
+               <field type="float" name="eph">GPS HDOP</field>
+               <field type="float" name="epv">GPS VDOP</field>
+               <field type="float" name="v">GPS ground speed</field>
+               <field type="float" name="hdg">Compass heading in degrees, 0..360 degrees</field>
+          </message>
+          <message id="34" name="SYS_STATUS">
+               <description>The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows wether the system is currently active or not and if an emergency occured. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occured it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout.</description>
+               <field type="uint8_t" name="mode">System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h</field>
+               <field type="uint8_t" name="nav_mode">Navigation mode, see MAV_NAV_MODE ENUM</field>
+               <field type="uint8_t" name="status">System status flag, see MAV_STATUS ENUM</field>
+               <field type="uint16_t" name="load">Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000</field>
+               <field type="uint16_t" name="vbat">Battery voltage, in millivolts (1 = 1 millivolt)</field>
+               <field type="uint16_t" name="battery_remaining">Remaining battery energy: (0%: 0, 100%: 1000)</field>
+               <field type="uint16_t" name="packet_drop">Dropped packets (packets that were corrupted on reception on the MAV)</field>
+          </message>
+          <message id="35" name="RC_CHANNELS_RAW">
+               <description>The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification.</description>
+               <field type="uint16_t" name="chan1_raw">RC channel 1 value, in microseconds</field>
+               <field type="uint16_t" name="chan2_raw">RC channel 2 value, in microseconds</field>
+               <field type="uint16_t" name="chan3_raw">RC channel 3 value, in microseconds</field>
+               <field type="uint16_t" name="chan4_raw">RC channel 4 value, in microseconds</field>
+               <field type="uint16_t" name="chan5_raw">RC channel 5 value, in microseconds</field>
+               <field type="uint16_t" name="chan6_raw">RC channel 6 value, in microseconds</field>
+               <field type="uint16_t" name="chan7_raw">RC channel 7 value, in microseconds</field>
+               <field type="uint16_t" name="chan8_raw">RC channel 8 value, in microseconds</field>
+               <field type="uint8_t" name="rssi">Receive signal strength indicator, 0: 0%, 255: 100%</field>
+          </message>
+          <message id="36" name="RC_CHANNELS_SCALED">
+               <description>The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000</description>
+               <field type="int16_t" name="chan1_scaled">RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
+               <field type="int16_t" name="chan2_scaled">RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
+               <field type="int16_t" name="chan3_scaled">RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
+               <field type="int16_t" name="chan4_scaled">RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
+               <field type="int16_t" name="chan5_scaled">RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
+               <field type="int16_t" name="chan6_scaled">RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
+               <field type="int16_t" name="chan7_scaled">RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
+               <field type="int16_t" name="chan8_scaled">RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000</field>
+               <field type="uint8_t" name="rssi">Receive signal strength indicator, 0: 0%, 255: 100%</field>
+          </message>
+          <message id="37" name="SERVO_OUTPUT_RAW">
+               <description>The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%.</description>
+               <field type="uint16_t" name="servo1_raw">Servo output 1 value, in microseconds</field>
+               <field type="uint16_t" name="servo2_raw">Servo output 2 value, in microseconds</field>
+               <field type="uint16_t" name="servo3_raw">Servo output 3 value, in microseconds</field>
+               <field type="uint16_t" name="servo4_raw">Servo output 4 value, in microseconds</field>
+               <field type="uint16_t" name="servo5_raw">Servo output 5 value, in microseconds</field>
+               <field type="uint16_t" name="servo6_raw">Servo output 6 value, in microseconds</field>
+               <field type="uint16_t" name="servo7_raw">Servo output 7 value, in microseconds</field>
+               <field type="uint16_t" name="servo8_raw">Servo output 8 value, in microseconds</field>
+          </message>
+          <message id="39" name="WAYPOINT">
+               <description>Message encoding a waypoint. This message is emitted to announce
      the presence of a waypoint and to set a waypoint on the system. The waypoint can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed, global frame is Z-up, right handed</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-     <field name="seq" type="uint16_t">Sequence</field>
-     <field name="frame" type="uint8_t">The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h</field>
-     <field name="command" type="uint8_t">The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs</field>
-     <field name="current" type="uint8_t">false:0, true:1</field>
-     <field name="autocontinue" type="uint8_t">autocontinue to next wp</field>
-     <field name="param1" type="float">PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters</field>
-     <field name="param2" type="float">PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds</field>
-     <field name="param3" type="float">PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.</field>
-     <field name="param4" type="float">PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH</field>
-     <field name="x" type="float">PARAM5 / local: x position, global: latitude</field>
-     <field name="y" type="float">PARAM6 / y position: global: longitude</field>
-     <field name="z" type="float">PARAM7 / z position: global: altitude</field>
-   </message>
-
-   <message name="WAYPOINT_REQUEST" id="40">
-     <description>Request the information of the waypoint with the sequence number seq. The response of the system to this message should be a WAYPOINT message.</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-     <field name="seq" type="uint16_t">Sequence</field>
-   </message>
-
-  <message name="WAYPOINT_SET_CURRENT" id="41">
-     <description>Set the waypoint with sequence number seq as current waypoint. This means that the MAV will continue to this waypoint on the shortest path (not following the waypoints in-between).</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-     <field name="seq" type="uint16_t">Sequence</field>
-   </message>
-
-  <message name="WAYPOINT_CURRENT" id="42">
-     <description>Message that announces the sequence number of the current active waypoint. The MAV will fly towards this waypoint.</description>
-     <field name="seq" type="uint16_t">Sequence</field>
-   </message>
-
-  <message name="WAYPOINT_REQUEST_LIST" id="43">
-     <description>Request the overall list of waypoints from the system/component.</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-   </message>
-
-   <message name="WAYPOINT_COUNT" id="44">
-     <description>This message is emitted as response to WAYPOINT_REQUEST_LIST by the MAV. The GCS can then request the individual waypoints based on the knowledge of the total number of waypoints.</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-     <field name="count" type="uint16_t">Number of Waypoints in the Sequence</field>
-   </message>
-
-  <message name="WAYPOINT_CLEAR_ALL" id="45">
-     <description>Delete all waypoints at once.</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-   </message>
-
-   <message name="WAYPOINT_REACHED" id="46">
-     <description>A certain waypoint has been reached. The system will either hold this position (or circle on the orbit) or (if the autocontinue on the WP was set) continue to the next waypoint.</description>
-     <field name="seq" type="uint16_t">Sequence</field>
-   </message>
-
-   <message name="WAYPOINT_ACK" id="47">
-     <description>Ack message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero).</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-     <field name="type" type="uint8_t">0: OK, 1: Error</field>
-   </message>
-
-   <message name="GPS_SET_GLOBAL_ORIGIN" id="48">
-     <description>As local waypoints exist, the global waypoint reference allows to transform between the local coordinate frame and the global (GPS) coordinate frame. This can be necessary when e.g. in- and outdoor settings are connected and the MAV should move from in- to outdoor.</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-     <field name="latitude" type="int32_t">global position * 1E7</field>
-     <field name="longitude" type="int32_t">global position * 1E7</field>
-     <field name="altitude" type="int32_t">global position * 1000</field>
-   </message>
-   
-   <message name="GPS_LOCAL_ORIGIN_SET" id="49">
-    <description>Once the MAV sets a new GPS-Local correspondence, this message announces the origin (0,0,0) position</description>
-    <field name="latitude" type="int32_t">Latitude (WGS84), expressed as * 1E7</field>
-    <field name="longitude" type="int32_t">Longitude (WGS84), expressed as * 1E7</field>
-    <field name="altitude" type="int32_t">Altitude(WGS84), expressed as * 1000</field>
-  </message>
-
-   <message name="LOCAL_POSITION_SETPOINT_SET" id="50">
-     <description>Set the setpoint for a local position controller. This is the position in local coordinates the MAV should fly to. This message is sent by the path/waypoint planner to the onboard position controller. As some MAVs have a degree of freedom in yaw (e.g. all helicopters/quadrotors), the desired yaw angle is part of the message.</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-     <field name="x" type="float">x position</field>
-     <field name="y" type="float">y position</field>
-     <field name="z" type="float">z position</field>
-     <field name="yaw" type="float">Desired yaw angle</field>
-   </message>
-
-   <message name="LOCAL_POSITION_SETPOINT" id="51">
-     <description>Transmit the current local setpoint of the controller to other MAVs (collision avoidance) and to the GCS.</description>
-     <field name="x" type="float">x position</field>
-     <field name="y" type="float">y position</field>
-     <field name="z" type="float">z position</field>
-     <field name="yaw" type="float">Desired yaw angle</field>
-   </message>
-   
-   <message name="CONTROL_STATUS" id="52">
-     <field name="position_fix" type="uint8_t">Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix </field>
-     <field name="vision_fix" type="uint8_t">Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix </field>
-     <field name="gps_fix" type="uint8_t">GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix </field>
-     <field name="ahrs_health" type="uint8_t">Attitude estimation health: 0: poor, 255: excellent</field>
-     <field name="control_att" type="uint8_t">0: Attitude control disabled, 1: enabled</field>
-     <field name="control_pos_xy" type="uint8_t">0: X, Y position control disabled, 1: enabled</field>
-     <field name="control_pos_z" type="uint8_t">0: Z position control disabled, 1: enabled</field>
-     <field name="control_pos_yaw" type="uint8_t">0: Yaw angle control disabled, 1: enabled</field>
-   </message>
-   
-      <message name="SAFETY_SET_ALLOWED_AREA" id="53">
-     <description>Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations.</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-     <field name="frame" type="uint8_t">Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.</field>
-     <field name="p1x" type="float">x position 1 / Latitude 1</field>
-     <field name="p1y" type="float">y position 1 / Longitude 1</field>
-     <field name="p1z" type="float">z position 1 / Altitude 1</field>
-     <field name="p2x" type="float">x position 2 / Latitude 2</field>
-     <field name="p2y" type="float">y position 2 / Longitude 2</field>
-     <field name="p2z" type="float">z position 2 / Altitude 2</field>
-   </message>
-   
-   <message name="SAFETY_ALLOWED_AREA" id="54">
-     <description>Read out the safety zone the MAV currently assumes.</description>
-     <field name="frame" type="uint8_t">Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.</field>
-     <field name="p1x" type="float">x position 1 / Latitude 1</field>
-     <field name="p1y" type="float">y position 1 / Longitude 1</field>
-     <field name="p1z" type="float">z position 1 / Altitude 1</field>
-     <field name="p2x" type="float">x position 2 / Latitude 2</field>
-     <field name="p2y" type="float">y position 2 / Longitude 2</field>
-     <field name="p2z" type="float">z position 2 / Altitude 2</field>
-   </message>
-
-   <message name="ATTITUDE_CONTROLLER_OUTPUT" id="60">
-     <description>The output of the attitude controller. This output is the control response the controller currently generates and the attitude the MAV would take if it is under control of the attitude controller. The primary use of this message is to check the response and signs of the controller before the actual flight.</description>
-     <field name="enabled" type="uint8_t">1: enabled, 0: disabled</field>
-     <field name="roll" type="int8_t">Attitude roll: -128: -100%, 127: +100%</field>
-     <field name="pitch" type="int8_t">Attitude pitch: -128: -100%, 127: +100%</field>
-     <field name="yaw" type="int8_t">Attitude yaw: -128: -100%, 127: +100%</field>
-     <field name="thrust" type="int8_t">Attitude thrust: -128: -100%, 127: +100%</field>
-   </message>
-
-   <message name="POSITION_CONTROLLER_OUTPUT" id="61">
-     <description>The output of the position controller. The primary use of this message is to check the response and signs of the controller before the actual flight.</description>
-     <field name="enabled" type="uint8_t">1: enabled, 0: disabled</field>
-     <field name="x" type="int8_t">Position x: -128: -100%, 127: +100%</field>
-     <field name="y" type="int8_t">Position y: -128: -100%, 127: +100%</field>
-     <field name="z" type="int8_t">Position z: -128: -100%, 127: +100%</field>
-     <field name="yaw" type="int8_t">Position yaw: -128: -100%, 127: +100%</field>
-   </message>
-   
-      <message name="NAV_CONTROLLER_OUTPUT" id="62">
-     <description>Outputs of the APM navigation controller. The primary use of this message is to check the response and signs
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+               <field type="uint16_t" name="seq">Sequence</field>
+               <field type="uint8_t" name="frame">The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h</field>
+               <field type="uint8_t" name="command">The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs</field>
+               <field type="uint8_t" name="current">false:0, true:1</field>
+               <field type="uint8_t" name="autocontinue">autocontinue to next wp</field>
+               <field type="float" name="param1">PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters</field>
+               <field type="float" name="param2">PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds</field>
+               <field type="float" name="param3">PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.</field>
+               <field type="float" name="param4">PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH</field>
+               <field type="float" name="x">PARAM5 / local: x position, global: latitude</field>
+               <field type="float" name="y">PARAM6 / y position: global: longitude</field>
+               <field type="float" name="z">PARAM7 / z position: global: altitude</field>
+          </message>
+          <message id="40" name="WAYPOINT_REQUEST">
+               <description>Request the information of the waypoint with the sequence number seq. The response of the system to this message should be a WAYPOINT message.</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+               <field type="uint16_t" name="seq">Sequence</field>
+          </message>
+          <message id="41" name="WAYPOINT_SET_CURRENT">
+               <description>Set the waypoint with sequence number seq as current waypoint. This means that the MAV will continue to this waypoint on the shortest path (not following the waypoints in-between).</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+               <field type="uint16_t" name="seq">Sequence</field>
+          </message>
+          <message id="42" name="WAYPOINT_CURRENT">
+               <description>Message that announces the sequence number of the current active waypoint. The MAV will fly towards this waypoint.</description>
+               <field type="uint16_t" name="seq">Sequence</field>
+          </message>
+          <message id="43" name="WAYPOINT_REQUEST_LIST">
+               <description>Request the overall list of waypoints from the system/component.</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+          </message>
+          <message id="44" name="WAYPOINT_COUNT">
+               <description>This message is emitted as response to WAYPOINT_REQUEST_LIST by the MAV. The GCS can then request the individual waypoints based on the knowledge of the total number of waypoints.</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+               <field type="uint16_t" name="count">Number of Waypoints in the Sequence</field>
+          </message>
+          <message id="45" name="WAYPOINT_CLEAR_ALL">
+               <description>Delete all waypoints at once.</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+          </message>
+          <message id="46" name="WAYPOINT_REACHED">
+               <description>A certain waypoint has been reached. The system will either hold this position (or circle on the orbit) or (if the autocontinue on the WP was set) continue to the next waypoint.</description>
+               <field type="uint16_t" name="seq">Sequence</field>
+          </message>
+          <message id="47" name="WAYPOINT_ACK">
+               <description>Ack message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero).</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+               <field type="uint8_t" name="type">0: OK, 1: Error</field>
+          </message>
+          <message id="48" name="GPS_SET_GLOBAL_ORIGIN">
+               <description>As local waypoints exist, the global waypoint reference allows to transform between the local coordinate frame and the global (GPS) coordinate frame. This can be necessary when e.g. in- and outdoor settings are connected and the MAV should move from in- to outdoor.</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+               <field type="int32_t" name="latitude">global position * 1E7</field>
+               <field type="int32_t" name="longitude">global position * 1E7</field>
+               <field type="int32_t" name="altitude">global position * 1000</field>
+          </message>
+          <message id="49" name="GPS_LOCAL_ORIGIN_SET">
+               <description>Once the MAV sets a new GPS-Local correspondence, this message announces the origin (0,0,0) position</description>
+               <field type="int32_t" name="latitude">Latitude (WGS84), expressed as * 1E7</field>
+               <field type="int32_t" name="longitude">Longitude (WGS84), expressed as * 1E7</field>
+               <field type="int32_t" name="altitude">Altitude(WGS84), expressed as * 1000</field>
+          </message>
+          <message id="50" name="LOCAL_POSITION_SETPOINT_SET">
+               <description>Set the setpoint for a local position controller. This is the position in local coordinates the MAV should fly to. This message is sent by the path/waypoint planner to the onboard position controller. As some MAVs have a degree of freedom in yaw (e.g. all helicopters/quadrotors), the desired yaw angle is part of the message.</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+               <field type="float" name="x">x position</field>
+               <field type="float" name="y">y position</field>
+               <field type="float" name="z">z position</field>
+               <field type="float" name="yaw">Desired yaw angle</field>
+          </message>
+          <message id="51" name="LOCAL_POSITION_SETPOINT">
+               <description>Transmit the current local setpoint of the controller to other MAVs (collision avoidance) and to the GCS.</description>
+               <field type="float" name="x">x position</field>
+               <field type="float" name="y">y position</field>
+               <field type="float" name="z">z position</field>
+               <field type="float" name="yaw">Desired yaw angle</field>
+          </message>
+          <message id="52" name="CONTROL_STATUS">
+               <field type="uint8_t" name="position_fix">Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix </field>
+               <field type="uint8_t" name="vision_fix">Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix </field>
+               <field type="uint8_t" name="gps_fix">GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix </field>
+               <field type="uint8_t" name="ahrs_health">Attitude estimation health: 0: poor, 255: excellent</field>
+               <field type="uint8_t" name="control_att">0: Attitude control disabled, 1: enabled</field>
+               <field type="uint8_t" name="control_pos_xy">0: X, Y position control disabled, 1: enabled</field>
+               <field type="uint8_t" name="control_pos_z">0: Z position control disabled, 1: enabled</field>
+               <field type="uint8_t" name="control_pos_yaw">0: Yaw angle control disabled, 1: enabled</field>
+          </message>
+          <message id="53" name="SAFETY_SET_ALLOWED_AREA">
+               <description>Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations.</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+               <field type="uint8_t" name="frame">Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.</field>
+               <field type="float" name="p1x">x position 1 / Latitude 1</field>
+               <field type="float" name="p1y">y position 1 / Longitude 1</field>
+               <field type="float" name="p1z">z position 1 / Altitude 1</field>
+               <field type="float" name="p2x">x position 2 / Latitude 2</field>
+               <field type="float" name="p2y">y position 2 / Longitude 2</field>
+               <field type="float" name="p2z">z position 2 / Altitude 2</field>
+          </message>
+          <message id="54" name="SAFETY_ALLOWED_AREA">
+               <description>Read out the safety zone the MAV currently assumes.</description>
+               <field type="uint8_t" name="frame">Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.</field>
+               <field type="float" name="p1x">x position 1 / Latitude 1</field>
+               <field type="float" name="p1y">y position 1 / Longitude 1</field>
+               <field type="float" name="p1z">z position 1 / Altitude 1</field>
+               <field type="float" name="p2x">x position 2 / Latitude 2</field>
+               <field type="float" name="p2y">y position 2 / Longitude 2</field>
+               <field type="float" name="p2z">z position 2 / Altitude 2</field>
+          </message>
+          <message id="55" name="SET_ROLL_PITCH_YAW_THRUST">
+               <description>Set roll, pitch and yaw.</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+               <field type="float" name="roll">Desired roll angle in radians</field>
+               <field type="float" name="pitch">Desired pitch angle in radians</field>
+               <field type="float" name="yaw">Desired yaw angle in radians</field>
+               <field type="float" name="thrust">Collective thrust, normalized to 0 .. 1</field>
+          </message>
+          <message id="56" name="SET_ROLL_PITCH_YAW_SPEED_THRUST">
+               <description>Set roll, pitch and yaw.</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+               <field type="float" name="roll_speed">Desired roll angular speed in rad/s</field>
+               <field type="float" name="pitch_speed">Desired pitch angular speed in rad/s</field>
+               <field type="float" name="yaw_speed">Desired yaw angular speed in rad/s</field>
+               <field type="float" name="thrust">Collective thrust, normalized to 0 .. 1</field>
+          </message>
+          <message id="57" name="ROLL_PITCH_YAW_THRUST_SETPOINT">
+               <description>Setpoint in roll, pitch, yaw currently active on the system.</description>
+               <field type="uint64_t" name="time_us">Timestamp in micro seconds since unix epoch</field>
+               <field type="float" name="roll">Desired roll angle in radians</field>
+               <field type="float" name="pitch">Desired pitch angle in radians</field>
+               <field type="float" name="yaw">Desired yaw angle in radians</field>
+               <field type="float" name="thrust">Collective thrust, normalized to 0 .. 1</field>
+          </message>
+          <message id="58" name="ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT">
+               <description>Setpoint in rollspeed, pitchspeed, yawspeed currently active on the system.</description>
+               <field type="uint64_t" name="time_us">Timestamp in micro seconds since unix epoch</field>
+               <field type="float" name="roll_speed">Desired roll angular speed in rad/s</field>
+               <field type="float" name="pitch_speed">Desired pitch angular speed in rad/s</field>
+               <field type="float" name="yaw_speed">Desired yaw angular speed in rad/s</field>
+               <field type="float" name="thrust">Collective thrust, normalized to 0 .. 1</field>
+          </message>
+          <message id="62" name="NAV_CONTROLLER_OUTPUT">
+               <description>Outputs of the APM navigation controller. The primary use of this message is to check the response and signs
 of the controller before actual flight and to assist with tuning controller parameters </description>
-     <field name="nav_roll" type="float">Current desired roll in degrees</field>
-     <field name="nav_pitch" type="float">Current desired pitch in degrees</field>
-     <field name="nav_bearing" type="int16_t">Current desired heading in degrees</field>
-     <field name="target_bearing" type="int16_t">Bearing to current waypoint/target in degrees</field>
-     <field name="wp_dist" type="uint16_t">Distance to active waypoint in meters</field>
-     <field name="alt_error" type="float">Current altitude error in meters</field>
-     <field name="aspd_error" type="float">Current airspeed error in meters/second</field>
-     <field name="xtrack_error" type="float">Current crosstrack error on x-y plane in meters</field>
-   </message> 
-   
-   <message name="POSITION_TARGET" id="63">
-   <description>The goal position of the system. This position is the input to any navigation or path planning algorithm and does NOT represent the current controller setpoint.</description>
-     <field name="x" type="float">x position</field>
-     <field name="y" type="float">y position</field>
-     <field name="z" type="float">z position</field>
-     <field name="yaw" type="float">yaw orientation in radians, 0 = NORTH</field>
-   </message>
-   
-   <message name="STATE_CORRECTION" id="64">
-   <description>Corrects the systems state by adding an error correction term to the position and velocity, and by rotating the attitude by a correction angle.</description>
-     <field name="xErr" type="float">x position error</field>
-     <field name="yErr" type="float">y position error</field>
-     <field name="zErr" type="float">z position error</field>
-     <field name="rollErr" type="float">roll error (radians)</field>
-     <field name="pitchErr" type="float">pitch error (radians)</field>
-     <field name="yawErr" type="float">yaw error (radians)</field>
-     <field name="vxErr" type="float">x velocity</field>
-     <field name="vyErr" type="float">y velocity</field>
-     <field name="vzErr" type="float">z velocity</field>
-   </message>
-
-   <message name="SET_ALTITUDE" id="65">
-     <field name="target" type="uint8_t">The system setting the altitude</field>
-     <field name="mode" type="uint32_t">The new altitude in meters</field>
-   </message>
-
-  <message name="REQUEST_DATA_STREAM" id="66">
-     <field name="target_system" type="uint8_t">The target requested to send the message stream.</field>
-     <field name="target_component" type="uint8_t">The target requested to send the message stream.</field>
-     <field name="req_stream_id" type="uint8_t">The ID of the requested message type</field>
-     <field name="req_message_rate" type="uint16_t">The requested interval between two messages of this type</field>
-     <field name="start_stop" type="uint8_t">1 to start sending, 0 to stop sending.</field>
-   </message>
-
-   <message name="MANUAL_CONTROL" id="69">
-     <field name="target" type="uint8_t">The system to be controlled</field>
-     <field name="roll" type="float">roll</field>
-     <field name="pitch" type="float">pitch</field>
-     <field name="yaw" type="float">yaw</field>
-     <field name="thrust" type="float">thrust</field>
-     <field name="roll_manual" type="uint8_t">roll control enabled auto:0, manual:1</field>
-     <field name="pitch_manual" type="uint8_t">pitch auto:0, manual:1</field>
-     <field name="yaw_manual" type="uint8_t">yaw auto:0, manual:1</field>
-     <field name="thrust_manual" type="uint8_t">thrust auto:0, manual:1</field>
-   </message>
-
-   <message name="GLOBAL_POSITION_INT" id="73">
-     <description>The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up)</description>
-     <field name="lat"   type="int32_t">Latitude, expressed as * 1E7</field>
-     <field name="lon"   type="int32_t">Longitude, expressed as * 1E7</field>
-     <field name="alt"   type="int32_t">Altitude in meters, expressed as * 1000 (millimeters)</field>
-     <field name="vx"  type="int16_t">Ground X Speed (Latitude), expressed as m/s * 100</field>
-     <field name="vy"  type="int16_t">Ground Y Speed (Longitude), expressed as m/s * 100</field>
-     <field name="vz"  type="int16_t">Ground Z Speed (Altitude), expressed as m/s * 100</field>
-   </message>
-
-  <message name="VFR_HUD" id="74">
-     <description>Metrics typically displayed on a HUD for fixed wing aircraft</description>
-     <field name="airspeed" type="float">Current airspeed in m/s</field>
-     <field name="groundspeed" type="float">Current ground speed in m/s</field>
-     <field name="heading" type="int16_t">Current heading in degrees, in compass units (0..360, 0=north)</field>
-     <field name="throttle" type="uint16_t">Current throttle setting in integer percent, 0 to 100</field>
-     <field name="alt" type="float">Current altitude (MSL), in meters</field>
-     <field name="climb" type="float">Current climb rate in meters/second</field>
-   </message> 
-   
-   <message name="COMMAND" id="75">
-     <description>Send a command with up to four parameters to the MAV</description>
-     <field name="target_system" type="uint8_t">System which should execute the command</field>
-     <field name="target_component" type="uint8_t">Component which should execute the command, 0 for all components</field>
-     <field name="command" type="uint8_t">Command ID, as defined by MAV_CMD enum.</field>
-     <field name="confirmation" type="uint8_t">0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)</field>
-     <field name="param1" type="float">Parameter 1, as defined by MAV_CMD enum.</field>
-     <field name="param2" type="float">Parameter 2, as defined by MAV_CMD enum.</field>
-     <field name="param3" type="float">Parameter 3, as defined by MAV_CMD enum.</field>
-     <field name="param4" type="float">Parameter 4, as defined by MAV_CMD enum.</field>
-   </message>
-   
-   <message name="COMMAND_ACK" id="76">
-     <description>Report status of a command. Includes feedback wether the command was executed</description>
-     <field name="command" type="float">Current airspeed in m/s</field>
-     <field name="result" type="float">1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION</field>
-   </message>
-   
-   <!-- MESSAGE IDs 80 - 250: Space for custom messages in individual projectname_messages.xml files -->
-
-
-
-
-  <message name="DEBUG_VECT" id="251">
-     <field name="name" type="char[10]">Name</field>
-     <field name="usec" type="uint64_t">Timestamp</field>
-     <field name="x" type="float">x</field>
-     <field name="y" type="float">y</field>
-     <field name="z" type="float">z</field>
-   </message>
-
-<message name="NAMED_VALUE_FLOAT" id="252">
-     <description>Send a key-value pair as float. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output.</description>
-     <field name="name"  type="char[10]">Name of the debug variable</field>
-     <field name="value"  type="float">Floating point value</field>
-   </message>
-
-<message name="NAMED_VALUE_INT" id="253">
-     <description>Send a key-value pair as integer. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output.</description>
-     <field name="name"  type="char[10]">Name of the debug variable</field>
-     <field name="value"  type="int32_t">Signed integer value</field>
-   </message>
-
-  <message name="STATUSTEXT" id= "254">
-    <description>Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz).</description>
-    <field name="severity" type="uint8_t">Severity of status, 0 = info message, 255 = critical fault</field>
-    <field name="text" type="int8_t[50]">Status text message, without null termination character</field>
-   </message>
-
-<message name="DEBUG" id="255">
-     <description>Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N.</description>
-     <field name="ind"  type="uint8_t">index of debug variable</field>
-     <field name="value"  type="float">DEBUG value</field>
-   </message>
-
-</messages>
+               <field type="float" name="nav_roll">Current desired roll in degrees</field>
+               <field type="float" name="nav_pitch">Current desired pitch in degrees</field>
+               <field type="int16_t" name="nav_bearing">Current desired heading in degrees</field>
+               <field type="int16_t" name="target_bearing">Bearing to current waypoint/target in degrees</field>
+               <field type="uint16_t" name="wp_dist">Distance to active waypoint in meters</field>
+               <field type="float" name="alt_error">Current altitude error in meters</field>
+               <field type="float" name="aspd_error">Current airspeed error in meters/second</field>
+               <field type="float" name="xtrack_error">Current crosstrack error on x-y plane in meters</field>
+          </message>
+          <message id="63" name="POSITION_TARGET">
+               <description>The goal position of the system. This position is the input to any navigation or path planning algorithm and does NOT represent the current controller setpoint.</description>
+               <field type="float" name="x">x position</field>
+               <field type="float" name="y">y position</field>
+               <field type="float" name="z">z position</field>
+               <field type="float" name="yaw">yaw orientation in radians, 0 = NORTH</field>
+          </message>
+          <message id="64" name="STATE_CORRECTION">
+               <description>Corrects the systems state by adding an error correction term to the position and velocity, and by rotating the attitude by a correction angle.</description>
+               <field type="float" name="xErr">x position error</field>
+               <field type="float" name="yErr">y position error</field>
+               <field type="float" name="zErr">z position error</field>
+               <field type="float" name="rollErr">roll error (radians)</field>
+               <field type="float" name="pitchErr">pitch error (radians)</field>
+               <field type="float" name="yawErr">yaw error (radians)</field>
+               <field type="float" name="vxErr">x velocity</field>
+               <field type="float" name="vyErr">y velocity</field>
+               <field type="float" name="vzErr">z velocity</field>
+          </message>
+          <message id="65" name="SET_ALTITUDE">
+               <field type="uint8_t" name="target">The system setting the altitude</field>
+               <field type="uint32_t" name="mode">The new altitude in meters</field>
+          </message>
+          <message id="66" name="REQUEST_DATA_STREAM">
+               <field type="uint8_t" name="target_system">The target requested to send the message stream.</field>
+               <field type="uint8_t" name="target_component">The target requested to send the message stream.</field>
+               <field type="uint8_t" name="req_stream_id">The ID of the requested message type</field>
+               <field type="uint16_t" name="req_message_rate">Update rate in Hertz</field>
+               <field type="uint8_t" name="start_stop">1 to start sending, 0 to stop sending.</field>
+          </message>
+          <message id="67" name="HIL_STATE">
+               <description>This packet is useful for high throughput
+                applications such as hardware in the loop simulations.
+            </description>
+               <field type="uint64_t" name="usec">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
+               <field type="float" name="roll">Roll angle (rad)</field>
+               <field type="float" name="pitch">Pitch angle (rad)</field>
+               <field type="float" name="yaw">Yaw angle (rad)</field>
+               <field type="float" name="rollspeed">Roll angular speed (rad/s)</field>
+               <field type="float" name="pitchspeed">Pitch angular speed (rad/s)</field>
+               <field type="float" name="yawspeed">Yaw angular speed (rad/s)</field>
+               <field type="int32_t" name="lat">Latitude, expressed as * 1E7</field>
+               <field type="int32_t" name="lon">Longitude, expressed as * 1E7</field>
+               <field type="int32_t" name="alt">Altitude in meters, expressed as * 1000 (millimeters)</field>
+               <field type="int16_t" name="vx">Ground X Speed (Latitude), expressed as m/s * 100</field>
+               <field type="int16_t" name="vy">Ground Y Speed (Longitude), expressed as m/s * 100</field>
+               <field type="int16_t" name="vz">Ground Z Speed (Altitude), expressed as m/s * 100</field>
+               <field type="int16_t" name="xacc">X acceleration (mg)</field>
+               <field type="int16_t" name="yacc">Y acceleration (mg)</field>
+               <field type="int16_t" name="zacc">Z acceleration (mg)</field>
+          </message>
+          <message id="68" name="HIL_CONTROLS">
+               <description>Hardware in the loop control outputs</description>
+               <field type="uint64_t" name="time_us">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
+               <field type="float" name="roll_ailerons">Control output -3 .. 1</field>
+               <field type="float" name="pitch_elevator">Control output -1 .. 1</field>
+               <field type="float" name="yaw_rudder">Control output -1 .. 1</field>
+               <field type="float" name="throttle">Throttle 0 .. 1</field>
+               <field type="uint8_t" name="mode">System mode (MAV_MODE)</field>
+               <field type="uint8_t" name="nav_mode">Navigation mode (MAV_NAV_MODE)</field>
+          </message>
+          <message id="69" name="MANUAL_CONTROL">
+               <field type="uint8_t" name="target">The system to be controlled</field>
+               <field type="float" name="roll">roll</field>
+               <field type="float" name="pitch">pitch</field>
+               <field type="float" name="yaw">yaw</field>
+               <field type="float" name="thrust">thrust</field>
+               <field type="uint8_t" name="roll_manual">roll control enabled auto:0, manual:1</field>
+               <field type="uint8_t" name="pitch_manual">pitch auto:0, manual:1</field>
+               <field type="uint8_t" name="yaw_manual">yaw auto:0, manual:1</field>
+               <field type="uint8_t" name="thrust_manual">thrust auto:0, manual:1</field>
+          </message>
+          <message id="70" name="RC_CHANNELS_OVERRIDE">
+               <description>The RAW values of the RC channels sent to the MAV to override info received from the RC radio. A value of -1 means no change to that channel. A value of 0 means control of that channel should be released back to the RC radio. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification.</description>
+               <field type="uint8_t" name="target_system">System ID</field>
+               <field type="uint8_t" name="target_component">Component ID</field>
+               <field type="uint16_t" name="chan1_raw">RC channel 1 value, in microseconds</field>
+               <field type="uint16_t" name="chan2_raw">RC channel 2 value, in microseconds</field>
+               <field type="uint16_t" name="chan3_raw">RC channel 3 value, in microseconds</field>
+               <field type="uint16_t" name="chan4_raw">RC channel 4 value, in microseconds</field>
+               <field type="uint16_t" name="chan5_raw">RC channel 5 value, in microseconds</field>
+               <field type="uint16_t" name="chan6_raw">RC channel 6 value, in microseconds</field>
+               <field type="uint16_t" name="chan7_raw">RC channel 7 value, in microseconds</field>
+               <field type="uint16_t" name="chan8_raw">RC channel 8 value, in microseconds</field>
+          </message>
+          <message id="73" name="GLOBAL_POSITION_INT">
+               <description>The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up)</description>
+               <field type="int32_t" name="lat">Latitude, expressed as * 1E7</field>
+               <field type="int32_t" name="lon">Longitude, expressed as * 1E7</field>
+               <field type="int32_t" name="alt">Altitude in meters, expressed as * 1000 (millimeters)</field>
+               <field type="int16_t" name="vx">Ground X Speed (Latitude), expressed as m/s * 100</field>
+               <field type="int16_t" name="vy">Ground Y Speed (Longitude), expressed as m/s * 100</field>
+               <field type="int16_t" name="vz">Ground Z Speed (Altitude), expressed as m/s * 100</field>
+          </message>
+          <message id="74" name="VFR_HUD">
+               <description>Metrics typically displayed on a HUD for fixed wing aircraft</description>
+               <field type="float" name="airspeed">Current airspeed in m/s</field>
+               <field type="float" name="groundspeed">Current ground speed in m/s</field>
+               <field type="int16_t" name="heading">Current heading in degrees, in compass units (0..360, 0=north)</field>
+               <field type="uint16_t" name="throttle">Current throttle setting in integer percent, 0 to 100</field>
+               <field type="float" name="alt">Current altitude (MSL), in meters</field>
+               <field type="float" name="climb">Current climb rate in meters/second</field>
+          </message>
+          <message id="75" name="COMMAND">
+               <description>Send a command with up to four parameters to the MAV</description>
+               <field type="uint8_t" name="target_system">System which should execute the command</field>
+               <field type="uint8_t" name="target_component">Component which should execute the command, 0 for all components</field>
+               <field type="uint8_t" name="command">Command ID, as defined by MAV_CMD enum.</field>
+               <field type="uint8_t" name="confirmation">0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)</field>
+               <field type="float" name="param1">Parameter 1, as defined by MAV_CMD enum.</field>
+               <field type="float" name="param2">Parameter 2, as defined by MAV_CMD enum.</field>
+               <field type="float" name="param3">Parameter 3, as defined by MAV_CMD enum.</field>
+               <field type="float" name="param4">Parameter 4, as defined by MAV_CMD enum.</field>
+          </message>
+          <message id="76" name="COMMAND_ACK">
+               <description>Report status of a command. Includes feedback wether the command was executed</description>
+               <field type="float" name="command">Current airspeed in m/s</field>
+               <field type="float" name="result">1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION</field>
+          </message>
+          <message id="100" name="OPTICAL_FLOW">
+               <description>Optical flow from a flow sensor (e.g. optical mouse sensor)</description>
+               <field type="uint64_t" name="time">Timestamp (UNIX)</field>
+               <field type="uint8_t" name="sensor_id">Sensor ID</field>
+               <field type="int16_t" name="flow_x">Flow in pixels in x-sensor direction</field>
+               <field type="int16_t" name="flow_y">Flow in pixels in y-sensor direction</field>
+               <field type="uint8_t" name="quality">Optical flow quality / confidence. 0: bad, 255: maximum quality</field>
+               <field type="float" name="ground_distance">Ground distance in meters</field>
+          </message>
+          <message id="140" name="OBJECT_DETECTION_EVENT">
+               <description>Object has been detected</description>
+               <field type="uint32_t" name="time">Timestamp in milliseconds since system boot</field>
+               <field type="uint16_t" name="object_id">Object ID</field>
+               <field type="uint8_t" name="type">Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal</field>
+               <field type="char[20]" name="name">Name of the object as defined by the detector</field>
+               <field type="uint8_t" name="quality">Detection quality / confidence. 0: bad, 255: maximum confidence</field>
+               <field type="float" name="bearing">Angle of the object with respect to the body frame in NED coordinates in radians. 0: front</field>
+               <field type="float" name="distance">Ground distance in meters</field>
+          </message>
+          <!-- MESSAGE IDs 80 - 250: Space for custom messages in individual projectname_messages.xml files -->
+          <message id="251" name="DEBUG_VECT">
+               <field type="char[10]" name="name">Name</field>
+               <field type="uint64_t" name="usec">Timestamp</field>
+               <field type="float" name="x">x</field>
+               <field type="float" name="y">y</field>
+               <field type="float" name="z">z</field>
+          </message>
+          <message id="252" name="NAMED_VALUE_FLOAT">
+               <description>Send a key-value pair as float. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output.</description>
+               <field type="char[10]" name="name">Name of the debug variable</field>
+               <field type="float" name="value">Floating point value</field>
+          </message>
+          <message id="253" name="NAMED_VALUE_INT">
+               <description>Send a key-value pair as integer. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output.</description>
+               <field type="char[10]" name="name">Name of the debug variable</field>
+               <field type="int32_t" name="value">Signed integer value</field>
+          </message>
+          <message id="254" name="STATUSTEXT">
+               <description>Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz).</description>
+               <field type="uint8_t" name="severity">Severity of status, 0 = info message, 255 = critical fault</field>
+               <field type="int8_t[50]" name="text">Status text message, without null termination character</field>
+          </message>
+          <message id="255" name="DEBUG">
+               <description>Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N.</description>
+               <field type="uint8_t" name="ind">index of debug variable</field>
+               <field type="float" name="value">DEBUG value</field>
+          </message>
+     </messages>
 </mavlink>
diff --git a/thirdParty/mavlink/message_definitions/minimal.xml b/thirdParty/mavlink/message_definitions/minimal.xml
index 16d26831e165d7c8ff3410cbab22accfd34311ca..5b41a4909090112233c5b91d1fa3067d7ffbdcbf 100644
--- a/thirdParty/mavlink/message_definitions/minimal.xml
+++ b/thirdParty/mavlink/message_definitions/minimal.xml
@@ -1,13 +1,13 @@
 <?xml version='1.0'?>
 <mavlink>
-     <version>2</version>
-     <enums/>
-     <messages>
-          <message id="0" name="HEARTBEAT">
-               <description>The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot).</description>
-               <field type="uint8_t" name="type">Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)</field>
-               <field type="uint8_t" name="autopilot">Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM</field>
-               <field type="uint8_t_mavlink_version" name="mavlink_version">MAVLink version</field>
-          </message>
-     </messages>
+    <version>2</version>
+    <enums/>
+    <messages>
+        <message id="0" name="HEARTBEAT">
+            <description>The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot).</description>
+            <field type="uint8_t" name="type">Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)</field>
+            <field type="uint8_t" name="autopilot">Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM</field>
+            <field type="uint8_t_mavlink_version" name="mavlink_version">MAVLink version</field>
+        </message>
+    </messages>
 </mavlink>
diff --git a/thirdParty/mavlink/message_definitions/pixhawk.xml b/thirdParty/mavlink/message_definitions/pixhawk.xml
index e2a99a738b666758d65670e47de52f181422b5ac..7bed667831bbd9a119ceda537f25dfdd12d7db33 100644
--- a/thirdParty/mavlink/message_definitions/pixhawk.xml
+++ b/thirdParty/mavlink/message_definitions/pixhawk.xml
@@ -1,262 +1,276 @@
 <?xml version="1.0"?>
 <mavlink>
-<include>common.xml</include>
+    <include>common.xml</include>
+    
+    <enums>
+        <enum name="DATA_TYPES">
+            <description>Content Types for data transmission handshake</description>
+            <entry name = "DATA_TYPE_JPEG_IMAGE" value="1">
+            </entry>
+            <entry name = "DATA_TYPE_RAW_IMAGE" value="2">
+            </entry>
+            <entry name = "DATA_TYPE_KINECT" value="3">
+            </entry>
+        </enum>
+    </enums>
+    
+    <messages>
+        <message name="ATTITUDE_CONTROL" id="85">
+            <field name="target" type="uint8_t">The system to be controlled</field>
+            <field name="roll" type="float">roll</field>
+            <field name="pitch" type="float">pitch</field>
+            <field name="yaw" type="float">yaw</field>
+            <field name="thrust" type="float">thrust</field>
+            <field name="roll_manual" type="uint8_t">roll control enabled auto:0, manual:1</field>
+            <field name="pitch_manual" type="uint8_t">pitch auto:0, manual:1</field>
+            <field name="yaw_manual" type="uint8_t">yaw auto:0, manual:1</field>
+            <field name="thrust_manual" type="uint8_t">thrust auto:0, manual:1</field>
+        </message>
+        
+        <message name="SET_CAM_SHUTTER" id="100">
+            <field name="cam_no" type="uint8_t">Camera id</field>
+            <field name="cam_mode" type="uint8_t">Camera mode: 0 = auto, 1 = manual</field>
+            <field name="trigger_pin" type="uint8_t">Trigger pin, 0-3 for PtGrey FireFly</field>
+            <field name="interval" type="uint16_t">Shutter interval, in microseconds</field>
+            <field name="exposure" type="uint16_t">Exposure time, in microseconds</field>
+            <field name="gain" type="float">Camera gain</field>
+        </message>
+        
+        <message name="IMAGE_TRIGGERED" id="101">
+            <field name="timestamp" type="uint64_t">Timestamp</field>
+            <field name="seq" type="uint32_t">IMU seq</field>
+            <field name="roll" type="float">Roll angle in rad</field>
+            <field name="pitch" type="float">Pitch angle in rad</field>
+            <field name="yaw" type="float">Yaw angle in rad</field>
+            <field name="local_z" type="float">Local frame Z coordinate (height over ground)</field>
+            <field name="lat" type="float">GPS X coordinate</field>
+            <field name="lon" type="float">GPS Y coordinate</field>
+            <field name="alt" type="float">Global frame altitude</field>
+            <field name="ground_x" type="float">Ground truth X</field>
+            <field name="ground_y" type="float">Ground truth Y</field>
+            <field name="ground_z" type="float">Ground truth Z</field>
+        </message>
+        
+        <message name="IMAGE_TRIGGER_CONTROL" id="102">
+            <field name="enable" type="uint8_t">0 to disable, 1 to enable</field>
+        </message>
+        
+        <message name="IMAGE_AVAILABLE" id="103">
+            <field name="cam_id" type="uint64_t">Camera id</field>
+            <field name="cam_no" type="uint8_t">Camera # (starts with 0)</field>
+            <field name="timestamp" type="uint64_t">Timestamp</field>
+            <field name="valid_until" type="uint64_t">Until which timestamp this buffer will stay valid</field>
+            <field name="img_seq" type="uint32_t">The image sequence number</field>
+            <field name="img_buf_index" type="uint32_t">Position of the image in the buffer, starts with 0</field>
+            <field name="width" type="uint16_t">Image width</field>
+            <field name="height" type="uint16_t">Image height</field>
+            <field name="depth" type="uint16_t">Image depth</field>
+            <field name="channels" type="uint8_t">Image channels</field>
+            <field name="key" type="uint32_t">Shared memory area key</field>
+            <field name="exposure" type="uint32_t">Exposure time, in microseconds</field>
+            <field name="gain" type="float">Camera gain</field>
+            <field name="roll" type="float">Roll angle in rad</field>
+            <field name="pitch" type="float">Pitch angle in rad</field>
+            <field name="yaw" type="float">Yaw angle in rad</field>
+            <field name="local_z" type="float">Local frame Z coordinate (height over ground)</field>
+            <field name="lat" type="float">GPS X coordinate</field>
+            <field name="lon" type="float">GPS Y coordinate</field>
+            <field name="alt" type="float">Global frame altitude</field>
+            <field name="ground_x" type="float">Ground truth X</field>
+            <field name="ground_y" type="float">Ground truth Y</field>
+            <field name="ground_z" type="float">Ground truth Z</field>
+        </message>
+        
+        <message name="VISION_POSITION_ESTIMATE" id="111">
+            <field name="usec" type="uint64_t">Timestamp (milliseconds)</field>
+            <field name="x"   type="float">Global X position</field>
+            <field name="y"   type="float">Global Y position</field>
+            <field name="z"   type="float">Global Z position</field>
+            <field name="roll" type="float">Roll angle in rad</field>
+            <field name="pitch" type="float">Pitch angle in rad</field>
+            <field name="yaw" type="float">Yaw angle in rad</field>
+        </message>
+        
+        <message name="VICON_POSITION_ESTIMATE" id="112">
+            <field name="usec" type="uint64_t">Timestamp (milliseconds)</field>
+            <field name="x"   type="float">Global X position</field>
+            <field name="y"   type="float">Global Y position</field>
+            <field name="z"   type="float">Global Z position</field>
+            <field name="roll" type="float">Roll angle in rad</field>
+            <field name="pitch" type="float">Pitch angle in rad</field>
+            <field name="yaw" type="float">Yaw angle in rad</field>
+        </message>
+        
+        <message name="VISION_SPEED_ESTIMATE" id="113">
+            <field name="usec" type="uint64_t">Timestamp (milliseconds)</field>
+            <field name="x"   type="float">Global X speed</field>
+            <field name="y"   type="float">Global Y speed</field>
+            <field name="z"   type="float">Global Z speed</field>
+        </message>
+        
+        <message name="POSITION_CONTROL_SETPOINT_SET" id="120">
+            <description>Message sent to the MAV to set a new position as reference for the controller</description>
+            <field name="target_system" type="uint8_t">System ID</field>
+            <field name="target_component" type="uint8_t">Component ID</field>
+            <field name="id" type="uint16_t">ID of waypoint, 0 for plain position</field>
+            <field name="x" type="float">x position</field>
+            <field name="y" type="float">y position</field>
+            <field name="z" type="float">z position</field>
+            <field name="yaw" type="float">yaw orientation in radians, 0 = NORTH</field>
+        </message>
+        
+        <message name="POSITION_CONTROL_OFFSET_SET" id="154">
+            <description>Message sent to the MAV to set a new offset from the currently controlled position</description>
+            <field name="target_system" type="uint8_t">System ID</field>
+            <field name="target_component" type="uint8_t">Component ID</field>
+            <field name="x" type="float">x position offset</field>
+            <field name="y" type="float">y position offset</field>
+            <field name="z" type="float">z position offset</field>
+            <field name="yaw" type="float">yaw orientation offset in radians, 0 = NORTH</field>
+        </message>
+        
+        <!-- Message sent by the MAV once it sets a new position as reference in the controller -->
+        <message name="POSITION_CONTROL_SETPOINT" id="121">
+            <field name="id" type="uint16_t">ID of waypoint, 0 for plain position</field>
+            <field name="x" type="float">x position</field>
+            <field name="y" type="float">y position</field>
+            <field name="z" type="float">z position</field>
+            <field name="yaw" type="float">yaw orientation in radians, 0 = NORTH</field>
+        </message>
+        
+        <message name="MARKER" id="130">
+            <field name="id" type="uint16_t">ID</field>
+            <field name="x" type="float">x position</field>
+            <field name="y" type="float">y position</field>
+            <field name="z" type="float">z position</field>
+            <field name="roll" type="float">roll orientation</field>
+            <field name="pitch" type="float">pitch orientation</field>
+            <field name="yaw" type="float">yaw orientation</field>
+        </message>
+        
+        <message name="RAW_AUX" id="141">
+            <field name="adc1" type="uint16_t">ADC1 (J405 ADC3, LPC2148 AD0.6)</field>
+            <field name="adc2" type="uint16_t">ADC2 (J405 ADC5, LPC2148 AD0.2)</field>
+            <field name="adc3" type="uint16_t">ADC3 (J405 ADC6, LPC2148 AD0.1)</field>
+            <field name="adc4" type="uint16_t">ADC4 (J405 ADC7, LPC2148 AD1.3)</field>
+            <field name="vbat" type="uint16_t">Battery voltage</field>
+            <field name="temp"  type="int16_t">Temperature (degrees celcius)</field>
+            <field name="baro"  type="int32_t">Barometric pressure (hecto Pascal)</field>
+        </message>
+        
+        <message name="AUX_STATUS" id="142">
+            <field name="load" type="uint16_t">Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000</field>
+            <field name="i2c0_err_count" type="uint16_t">Number of I2C errors since startup</field>
+            <field name="i2c1_err_count" type="uint16_t">Number of I2C errors since startup</field>
+            <field name="spi0_err_count" type="uint16_t">Number of I2C errors since startup</field>
+            <field name="spi1_err_count" type="uint16_t">Number of I2C errors since startup</field>
+            <field name="uart_total_err_count" type="uint16_t">Number of I2C errors since startup</field>
+        </message>
+        
+        <message name="WATCHDOG_HEARTBEAT" id="150">
+            <field name="watchdog_id" type="uint16_t">Watchdog ID</field>
+            <field name="process_count" type="uint16_t">Number of processes</field>
+        </message>
+        
+        <message name="WATCHDOG_PROCESS_INFO" id="151">
+            <field name="watchdog_id" type="uint16_t">Watchdog ID</field>
+            <field name="process_id" type="uint16_t">Process ID</field>
+            <field name="name" type="array[100]">Process name</field>
+            <field name="arguments" type="array[147]">Process arguments</field>
+            <field name="timeout" type="int32_t">Timeout (seconds)</field>
+        </message>
+        
+        <message name="WATCHDOG_PROCESS_STATUS" id="152">
+            <field name="watchdog_id" type="uint16_t">Watchdog ID</field>
+            <field name="process_id" type="uint16_t">Process ID</field>
+            <field name="state" type="uint8_t">Is running / finished / suspended / crashed</field>
+            <field name="muted" type="uint8_t">Is muted</field>
+            <field name="pid" type="int32_t">PID</field>
+            <field name="crashes" type="uint16_t">Number of crashes</field>
+        </message>
+        
+        <message name="WATCHDOG_COMMAND" id="153">
+            <field name="target_system_id" type="uint8_t">Target system ID</field>
+            <field name="watchdog_id" type="uint16_t">Watchdog ID</field>
+            <field name="process_id" type="uint16_t">Process ID</field>
+            <field name="command_id" type="uint8_t">Command ID</field>
+        </message>
+        
+        <message name="PATTERN_DETECTED" id="160">
+            <field name="type" type="uint8_t">0: Pattern, 1: Letter</field>
+            <field name="confidence" type="float">Confidence of detection</field>
+            <field name="file" type="array[100]">Pattern file name</field>
+            <field name="detected" type="uint8_t">Accepted as true detection, 0 no, 1 yes</field>
+        </message>
+        
+        <message name="POINT_OF_INTEREST" id="161">
+            <description>Notifies the operator about a point of interest (POI). This can be anything detected by the
+                system. This generic message is intented to help interfacing to generic visualizations and to display
+                the POI on a map.
+            </description>
+            <field name="type" type="uint8_t">0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug</field>
+            <field name="color" type="uint8_t">0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta</field>
+            <field name="coordinate_system" type="uint8_t">0: global, 1:local</field>
+            <field name="timeout" type="uint16_t">0: no timeout, >1: timeout in seconds</field>
+            <field name="x" type="float">X Position</field>
+            <field name="y" type="float">Y Position</field>
+            <field name="z" type="float">Z Position</field>
+            <field name="name" type="array[25]">POI name</field>
+        </message>
+        
+        <message name="POINT_OF_INTEREST_CONNECTION" id="162">
+            <description>Notifies the operator about the connection of two point of interests (POI). This can be anything detected by the
+                system. This generic message is intented to help interfacing to generic visualizations and to display
+                the POI on a map.
+            </description>
+            <field name="type" type="uint8_t">0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug</field>
+            <field name="color" type="uint8_t">0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta</field>
+            <field name="coordinate_system" type="uint8_t">0: global, 1:local</field>
+            <field name="timeout" type="uint16_t">0: no timeout, >1: timeout in seconds</field>
+            <field name="xp1" type="float">X1 Position</field>
+            <field name="yp1" type="float">Y1 Position</field>
+            <field name="zp1" type="float">Z1 Position</field>
+            <field name="xp2" type="float">X2 Position</field>
+            <field name="yp2" type="float">Y2 Position</field>
+            <field name="zp2" type="float">Z2 Position</field>
+            <field name="name" type="array[25]">POI connection name</field>
+        </message>
+        
+        <message name="DATA_TRANSMISSION_HANDSHAKE" id="170">
+            <field name="type" type="uint8_t">type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h)</field>
+            <field name="size" type="uint32_t">total data size in bytes (set on ACK only)</field>
+            <field name="packets" type="uint8_t">number of packets beeing sent (set on ACK only)</field>
+            <field name="payload" type="uint8_t">payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only)</field>
+            <field name="jpg_quality" type="uint8_t">JPEG quality out of [1,100]</field>
+        </message>
+        
+        <message name="ENCAPSULATED_DATA" id="171">
+            <field name="seqnr" type="uint16_t">sequence number (starting with 0 on every transmission)</field>
+            <field name="data" type="uint8_t[253]">image data bytes</field>
+        </message>
+        
+        <message name="BRIEF_FEATURE" id="172">
+            <field name="x" type="float">x position in m</field>
+            <field name="y" type="float">y position in m</field>
+            <field name="z" type="float">z position in m</field>
+            <field name="orientation_assignment" type="uint8_t">Orientation assignment 0: false, 1:true</field>
+            <field name="size" type="uint16_t">Size in pixels</field>
+            <field name="orientation" type="uint16_t">Orientation</field>
+            <field name="descriptor" type="uint8_t[32]">Descriptor</field>
+            <field name="response" type="float">Harris operator response at this location</field>
+        </message>
 
-<enums>
-   <enum name="DATA_TYPES">
-      <description>Content Types for data transmission handshake</description>
-      <entry name = "DATA_TYPE_JPEG_IMAGE" value="1">
-      </entry>
-      <entry name = "DATA_TYPE_RAW_IMAGE" value="2">
-      </entry>
-      <entry name = "DATA_TYPE_KINECT" value="3">
-      </entry>
-   </enum>
-</enums>
-
-<messages>
-  <message name="ATTITUDE_CONTROL" id="85">
-     <field name="target" type="uint8_t">The system to be controlled</field>
-     <field name="roll" type="float">roll</field>
-     <field name="pitch" type="float">pitch</field>
-     <field name="yaw" type="float">yaw</field>
-     <field name="thrust" type="float">thrust</field>
-     <field name="roll_manual" type="uint8_t">roll control enabled auto:0, manual:1</field>
-     <field name="pitch_manual" type="uint8_t">pitch auto:0, manual:1</field>
-     <field name="yaw_manual" type="uint8_t">yaw auto:0, manual:1</field>
-     <field name="thrust_manual" type="uint8_t">thrust auto:0, manual:1</field>
-   </message>
-
- <message name="SET_CAM_SHUTTER" id="100">
-     <field name="cam_no" type="uint8_t">Camera id</field>
-     <field name="cam_mode" type="uint8_t">Camera mode: 0 = auto, 1 = manual</field>
-     <field name="trigger_pin" type="uint8_t">Trigger pin, 0-3 for PtGrey FireFly</field>
-     <field name="interval" type="uint16_t">Shutter interval, in microseconds</field>
-     <field name="exposure" type="uint16_t">Exposure time, in microseconds</field>
-     <field name="gain" type="float">Camera gain</field>
-   </message>
-
-   <message name="IMAGE_TRIGGERED" id="101">
-     <field name="timestamp" type="uint64_t">Timestamp</field>
-     <field name="seq" type="uint32_t">IMU seq</field>
-     <field name="roll" type="float">Roll angle in rad</field>
-     <field name="pitch" type="float">Pitch angle in rad</field>
-     <field name="yaw" type="float">Yaw angle in rad</field>
-     <field name="local_z" type="float">Local frame Z coordinate (height over ground)</field>
-     <field name="lat" type="float">GPS X coordinate</field>
-     <field name="lon" type="float">GPS Y coordinate</field>
-     <field name="alt" type="float">Global frame altitude</field>
-     <field name="ground_x" type="float">Ground truth X</field>
-     <field name="ground_y" type="float">Ground truth Y</field>
-     <field name="ground_z" type="float">Ground truth Z</field>
-   </message>
-
-   <message name="IMAGE_TRIGGER_CONTROL" id="102">
-     <field name="enable" type="uint8_t">0 to disable, 1 to enable</field>
-   </message>
-
-   <message name="IMAGE_AVAILABLE" id="103">
-     <field name="cam_id" type="uint64_t">Camera id</field>
-     <field name="cam_no" type="uint8_t">Camera # (starts with 0)</field>
-     <field name="timestamp" type="uint64_t">Timestamp</field>
-     <field name="valid_until" type="uint64_t">Until which timestamp this buffer will stay valid</field>
-     <field name="img_seq" type="uint32_t">The image sequence number</field>
-     <field name="img_buf_index" type="uint32_t">Position of the image in the buffer, starts with 0</field>
-     <field name="width" type="uint16_t">Image width</field>
-     <field name="height" type="uint16_t">Image height</field>
-     <field name="depth" type="uint16_t">Image depth</field>
-     <field name="channels" type="uint8_t">Image channels</field>
-     <field name="key" type="uint32_t">Shared memory area key</field>
-     <field name="exposure" type="uint32_t">Exposure time, in microseconds</field>
-     <field name="gain" type="float">Camera gain</field>
-     <field name="roll" type="float">Roll angle in rad</field>
-     <field name="pitch" type="float">Pitch angle in rad</field>
-     <field name="yaw" type="float">Yaw angle in rad</field>
-     <field name="local_z" type="float">Local frame Z coordinate (height over ground)</field>
-     <field name="lat" type="float">GPS X coordinate</field>
-     <field name="lon" type="float">GPS Y coordinate</field>
-     <field name="alt" type="float">Global frame altitude</field>
-     <field name="ground_x" type="float">Ground truth X</field>
-     <field name="ground_y" type="float">Ground truth Y</field>
-     <field name="ground_z" type="float">Ground truth Z</field>
-   </message>
-
-   <message name="VISION_POSITION_ESTIMATE" id="111">
-     <field name="usec" type="uint64_t">Timestamp (milliseconds)</field>
-     <field name="x"   type="float">Global X position</field>
-     <field name="y"   type="float">Global Y position</field>
-     <field name="z"   type="float">Global Z position</field>
-     <field name="roll" type="float">Roll angle in rad</field>
-     <field name="pitch" type="float">Pitch angle in rad</field>
-     <field name="yaw" type="float">Yaw angle in rad</field>
-   </message>
-
-   <message name="VICON_POSITION_ESTIMATE" id="112">
-     <field name="usec" type="uint64_t">Timestamp (milliseconds)</field>
-     <field name="x"   type="float">Global X position</field>
-     <field name="y"   type="float">Global Y position</field>
-     <field name="z"   type="float">Global Z position</field>
-     <field name="roll" type="float">Roll angle in rad</field>
-     <field name="pitch" type="float">Pitch angle in rad</field>
-     <field name="yaw" type="float">Yaw angle in rad</field>
-   </message>
-
-   <message name="VISION_SPEED_ESTIMATE" id="113">
-     <field name="usec" type="uint64_t">Timestamp (milliseconds)</field>
-     <field name="x"   type="float">Global X speed</field>
-     <field name="y"   type="float">Global Y speed</field>
-     <field name="z"   type="float">Global Z speed</field>
-   </message>
-
-   <message name="POSITION_CONTROL_SETPOINT_SET" id="120">
-     <description>Message sent to the MAV to set a new position as reference for the controller</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-     <field name="id" type="uint16_t">ID of waypoint, 0 for plain position</field>
-     <field name="x" type="float">x position</field>
-     <field name="y" type="float">y position</field>
-     <field name="z" type="float">z position</field>
-     <field name="yaw" type="float">yaw orientation in radians, 0 = NORTH</field>
-   </message>
-
-   <message name="POSITION_CONTROL_OFFSET_SET" id="154">
-     <description>Message sent to the MAV to set a new offset from the currently controlled position</description>
-     <field name="target_system" type="uint8_t">System ID</field>
-     <field name="target_component" type="uint8_t">Component ID</field>
-     <field name="x" type="float">x position offset</field>
-     <field name="y" type="float">y position offset</field>
-     <field name="z" type="float">z position offset</field>
-     <field name="yaw" type="float">yaw orientation offset in radians, 0 = NORTH</field>
-   </message>
-
-  <!-- Message sent by the MAV once it sets a new position as reference in the controller -->
-   <message name="POSITION_CONTROL_SETPOINT" id="121">
-     <field name="id" type="uint16_t">ID of waypoint, 0 for plain position</field>
-     <field name="x" type="float">x position</field>
-     <field name="y" type="float">y position</field>
-     <field name="z" type="float">z position</field>
-     <field name="yaw" type="float">yaw orientation in radians, 0 = NORTH</field>
-   </message>
-
-   <message name="MARKER" id="130">
-     <field name="id" type="uint16_t">ID</field>
-     <field name="x" type="float">x position</field>
-     <field name="y" type="float">y position</field>
-     <field name="z" type="float">z position</field>
-     <field name="roll" type="float">roll orientation</field>
-     <field name="pitch" type="float">pitch orientation</field>
-     <field name="yaw" type="float">yaw orientation</field>
-   </message>
-
-   <message name="RAW_AUX" id="141">
-     <field name="adc1" type="uint16_t">ADC1 (J405 ADC3, LPC2148 AD0.6)</field>
-     <field name="adc2" type="uint16_t">ADC2 (J405 ADC5, LPC2148 AD0.2)</field>
-     <field name="adc3" type="uint16_t">ADC3 (J405 ADC6, LPC2148 AD0.1)</field>
-     <field name="adc4" type="uint16_t">ADC4 (J405 ADC7, LPC2148 AD1.3)</field>
-     <field name="vbat" type="uint16_t">Battery voltage</field>
-     <field name="temp"  type="int16_t">Temperature (degrees celcius)</field>
-     <field name="baro"  type="int32_t">Barometric pressure (hecto Pascal)</field>
-   </message>
-
-   <message name="AUX_STATUS" id="142">
-     <field name="load" type="uint16_t">Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000</field>
-     <field name="i2c0_err_count" type="uint16_t">Number of I2C errors since startup</field>
-<field name="i2c1_err_count" type="uint16_t">Number of I2C errors since startup</field>
-  <field name="spi0_err_count" type="uint16_t">Number of I2C errors since startup</field>
-<field name="spi1_err_count" type="uint16_t">Number of I2C errors since startup</field>
-  <field name="uart_total_err_count" type="uint16_t">Number of I2C errors since startup</field>
-   </message>
-
-  <message name="WATCHDOG_HEARTBEAT" id="150">
-    <field name="watchdog_id" type="uint16_t">Watchdog ID</field>
-    <field name="process_count" type="uint16_t">Number of processes</field>
-  </message>
-
-  <message name="WATCHDOG_PROCESS_INFO" id="151">
-    <field name="watchdog_id" type="uint16_t">Watchdog ID</field>
-    <field name="process_id" type="uint16_t">Process ID</field>
-    <field name="name" type="array[100]">Process name</field>
-    <field name="arguments" type="array[147]">Process arguments</field>
-    <field name="timeout" type="int32_t">Timeout (seconds)</field>
-  </message>
-
-  <message name="WATCHDOG_PROCESS_STATUS" id="152">
-    <field name="watchdog_id" type="uint16_t">Watchdog ID</field>
-    <field name="process_id" type="uint16_t">Process ID</field>
-    <field name="state" type="uint8_t">Is running / finished / suspended / crashed</field>
-    <field name="muted" type="uint8_t">Is muted</field>
-    <field name="pid" type="int32_t">PID</field>
-    <field name="crashes" type="uint16_t">Number of crashes</field>
-  </message>
-
-  <message name="WATCHDOG_COMMAND" id="153">
-    <field name="target_system_id" type="uint8_t">Target system ID</field>
-    <field name="watchdog_id" type="uint16_t">Watchdog ID</field>
-    <field name="process_id" type="uint16_t">Process ID</field>
-    <field name="command_id" type="uint8_t">Command ID</field>
-  </message>
-
-  <message name="PATTERN_DETECTED" id="160">
-    <field name="type" type="uint8_t">0: Pattern, 1: Letter</field>
-    <field name="confidence" type="float">Confidence of detection</field>
-    <field name="file" type="array[100]">Pattern file name</field>
-    <field name="detected" type="uint8_t">Accepted as true detection, 0 no, 1 yes</field>
-   </message>
-
-   <message name="POINT_OF_INTEREST" id="161">
-    <description>Notifies the operator about a point of interest (POI). This can be anything detected by the
-    system. This generic message is intented to help interfacing to generic visualizations and to display
-    the POI on a map.</description>
-    <field name="type" type="uint8_t">0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug</field>
-    <field name="color" type="uint8_t">0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta</field>
-    <field name="coordinate_system" type="uint8_t">0: global, 1:local</field>
-    <field name="timeout" type="uint16_t">0: no timeout, >1: timeout in seconds</field>
-    <field name="x" type="float">X Position</field>
-    <field name="y" type="float">Y Position</field>
-    <field name="z" type="float">Z Position</field>
-    <field name="name" type="array[25]">POI name</field>
-   </message>
-
-   <message name="POINT_OF_INTEREST_CONNECTION" id="162">
-    <description>Notifies the operator about the connection of two point of interests (POI). This can be anything detected by the
-    system. This generic message is intented to help interfacing to generic visualizations and to display
-    the POI on a map.</description>
-    <field name="type" type="uint8_t">0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug</field>
-    <field name="color" type="uint8_t">0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta</field>
-    <field name="coordinate_system" type="uint8_t">0: global, 1:local</field>
-    <field name="timeout" type="uint16_t">0: no timeout, >1: timeout in seconds</field>
-    <field name="xp1" type="float">X1 Position</field>
-    <field name="yp1" type="float">Y1 Position</field>
-    <field name="zp1" type="float">Z1 Position</field>
-    <field name="xp2" type="float">X2 Position</field>
-    <field name="yp2" type="float">Y2 Position</field>
-    <field name="zp2" type="float">Z2 Position</field>
-    <field name="name" type="array[25]">POI connection name</field>
-   </message>
-
-  <message name="DATA_TRANSMISSION_HANDSHAKE" id="170">
-    <field name="type" type="uint8_t">type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h)</field>
-    <field name="size" type="uint32_t">total data size in bytes (set on ACK only)</field>
-    <field name="packets" type="uint8_t">number of packets beeing sent (set on ACK only)</field>
-    <field name="payload" type="uint8_t">payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only)</field>
-    <field name="jpg_quality" type="uint8_t">JPEG quality out of [1,100]</field>
-   </message>
-
-  <message name="ENCAPSULATED_DATA" id="171">
-    <field name="seqnr" type="uint16_t">sequence number (starting with 0 on every transmission)</field>
-    <field name="data" type="uint8_t[253]">image data bytes</field>
-   </message>
-
-  <message name="BRIEF_FEATURE" id="172">
-    <field name="x" type="float">x position in m</field>
-    <field name="y" type="float">y position in m</field>
-    <field name="z" type="float">z position in m</field>
-    <field name="orientation_assignment" type="uint8_t">Orientation assignment 0: false, 1:true</field>
-    <field name="size" type="uint16_t">Size in pixels</field>
-    <field name="orientation" type="uint16_t">Orientation</field>
-    <field name="descriptor" type="uint8_t[32]">Descriptor</field>
-    <field name="response" type="float">Harris operator response at this location</field>
-  </message>
-
-</messages>
+        <message name="VISUAL_ODOMETRY" id="180">
+           <description>Visual odometry estimate describing relative pose between two frames</description>
+           <field name="frame1_time_us" type="uint64_t">Time at which frame 1 was captured (in microseconds since unix epoch)</field>
+           <field name="frame2_time_us" type="uint64_t">Time at which frame 2 was captured (in microseconds since unix epoch)</field>
+           <field name="x"   type="float">Relative X position</field>
+           <field name="y"   type="float">Relative Y position</field>
+           <field name="z"   type="float">Relative Z position</field>
+           <field name="roll" type="float">Relative roll angle in rad</field>
+           <field name="pitch" type="float">Relative pitch angle in rad</field>
+           <field name="yaw" type="float">Relative yaw angle in rad</field>
+       </message>
+        
+    </messages>
 </mavlink>
diff --git a/thirdParty/mavlink/message_definitions/slugs.xml b/thirdParty/mavlink/message_definitions/slugs.xml
index 6de0697fef69a78cafeb856f9b4569bb4ae14925..f8644c5c4be7e497178d892e9c15c25ba3c5131c 100644
--- a/thirdParty/mavlink/message_definitions/slugs.xml
+++ b/thirdParty/mavlink/message_definitions/slugs.xml
@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <mavlink>
-<include>common.xml</include>
-
-<!-- <enums> 
+    <include>common.xml</include>
+    
+    <!-- <enums> 
   <enum name="SLUGS_ACTION" > 
     <description> Slugs Actions </description> 
     <entry name = "SLUGS_ACTION_NONE"> 
@@ -33,110 +33,112 @@
   </enum>
   
 </enums> -->
-
-<messages>
-  
-   <message name="CPU_LOAD" id="170">
-     Sensor and DSC control loads.
-     <field name="sensLoad" type="uint8_t">Sensor DSC Load</field>
-     <field name="ctrlLoad" type="uint8_t">Control DSC Load</field>
-     <field name="batVolt" type="uint16_t">Battery Voltage in millivolts</field>     
-   </message>
-   
-   <message name="AIR_DATA" id="171">
-     Air data for altitude and airspeed computation.
-     <field name="dynamicPressure" type="float">Dynamic pressure (Pa)</field>
-     <field name="staticPressure" type="float">Static pressure (Pa)</field>
-     <field name="temperature" type="uint16_t">Board temperature</field>     
-   </message>
-   
-   <message name="SENSOR_BIAS" id="172">
-     Accelerometer and gyro biases.
-     <field name="axBias" type="float">Accelerometer X bias (m/s)</field>
-     <field name="ayBias" type="float">Accelerometer Y bias (m/s)</field>
-     <field name="azBias" type="float">Accelerometer Z bias (m/s)</field>
-     <field name="gxBias" type="float">Gyro X bias (rad/s)</field>
-     <field name="gyBias" type="float">Gyro Y bias (rad/s)</field>
-     <field name="gzBias" type="float">Gyro Z bias (rad/s)</field>
-   </message>
-   
-  <message name="DIAGNOSTIC" id="173">
-     Configurable diagnostic messages.
-     <field name="diagFl1" type="float">Diagnostic float 1</field>
-     <field name="diagFl2" type="float">Diagnostic float 2</field>
-     <field name="diagFl3" type="float">Diagnostic float 3</field>
-     <field name="diagSh1" type="int16_t">Diagnostic short 1</field>
-     <field name="diagSh2" type="int16_t">Diagnostic short 2</field>
-     <field name="diagSh3" type="int16_t">Diagnostic short 3</field>
-  </message>
-
-  <message name="SLUGS_NAVIGATION" id="176">
-     Data used in the navigation algorithm.
-     <field name="u_m" type="float">Measured Airspeed prior to the Nav Filter</field>
-     <field name="phi_c" type="float">Commanded Roll</field>
-     <field name="theta_c" type="float">Commanded Pitch</field>
-     <field name="psiDot_c" type="float">Commanded Turn rate</field>
-     <field name="ay_body" type="float">Y component of the body acceleration</field>
-     <field name="totalDist" type="float">Total Distance to Run on this leg of Navigation</field>
-     <field name="dist2Go" type="float">Remaining distance to Run on this leg of Navigation</field>
-     <field name="fromWP" type="uint8_t">Origin WP</field>
-     <field name="toWP" type="uint8_t">Destination WP</field>
-  </message>
-  
-  <message name="DATA_LOG" id="177">
-     Configurable data log probes to be used inside Simulink
-     <field name="fl_1" type="float">Log value 1 </field>
-     <field name="fl_2" type="float">Log value 2 </field>
-     <field name="fl_3" type="float">Log value 3 </field>
-     <field name="fl_4" type="float">Log value 4 </field>
-     <field name="fl_5" type="float">Log value 5 </field>
-     <field name="fl_6" type="float">Log value 6 </field>
-  </message>
-
-  <message name="GPS_DATE_TIME" id="179">
-     Pilot console PWM messges.
-     <field name="year"  type="uint8_t">Year reported by Gps </field>
-     <field name="month" type="uint8_t">Month reported by Gps </field>
-     <field name="day"   type="uint8_t">Day reported by Gps </field>
-     <field name="hour"  type="uint8_t">Hour reported by Gps </field>
-     <field name="min"   type="uint8_t">Min reported by Gps </field>
-     <field name="sec"   type="uint8_t">Sec reported by Gps  </field>
-     <field name="visSat"   type="uint8_t">Visible sattelites reported by Gps  </field>
-    </message>
     
-  <message name="MID_LVL_CMDS" id="180">
-     Mid Level commands sent from the GS to the autopilot. These are only sent when being opperated in mid-level commands mode from the ground; for periodic report of these commands generated from the autopilot see message XXXX.
-     <field name="target" type="uint8_t">The system setting the commands</field>
-     <field name="hCommand" type="float">Commanded Airspeed</field>
-     <field name="uCommand" type="float">Log value 2 </field>
-     <field name="rCommand" type="float">Log value 3 </field>
-  </message>    
-
-  <message name="CTRL_SRFC_PT" id="181">
-This message configures the Selective Passthrough mode. it allows to select which control surfaces the Pilot can control from his console. It is implemented as a bitfield as follows: 
-     Position          Bit        Code
-     =================================
-     15-8             Reserved  
-     7                 dt_pass    128
-     6                 dla_pass   64
-     5                 dra_pass   32
-     4                 dr_pass    16
-     3                 dle_pass   8
-     2                 dre_pass   4
-     1                 dlf_pass   2
-     0                 drf_pass   1
-    Where Bit 15 is the MSb. 0 = AP has control of the surface; 1 = Pilot Console has control of the surface.
-     <field name="target" type="uint8_t">The system setting the commands</field>
-     <field name="bitfieldPt" type="uint16_t">Bitfield containing the PT configuration</field>
-  </message>  
-  
-  
-  <message name="SLUGS_ACTION" id="183">
-     Action messages focused on the SLUGS AP. 
-     <field name="target" type="uint8_t">The system reporting the action</field>
-     <field name="actionId" type="uint8_t">Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names</field>
-     <field name="actionVal" type="uint16_t">Value associated with the action</field>
-  </message>
-
-</messages>
-</mavlink>
\ No newline at end of file
+    <messages>
+        
+        <message name="CPU_LOAD" id="170">
+            Sensor and DSC control loads.
+            <field name="sensLoad" type="uint8_t">Sensor DSC Load</field>
+            <field name="ctrlLoad" type="uint8_t">Control DSC Load</field>
+            <field name="batVolt" type="uint16_t">Battery Voltage in millivolts</field>     
+        </message>
+        
+        <message name="AIR_DATA" id="171">
+            Air data for altitude and airspeed computation.
+            <field name="dynamicPressure" type="float">Dynamic pressure (Pa)</field>
+            <field name="staticPressure" type="float">Static pressure (Pa)</field>
+            <field name="temperature" type="uint16_t">Board temperature</field>     
+        </message>
+        
+        <message name="SENSOR_BIAS" id="172">
+            Accelerometer and gyro biases.
+            <field name="axBias" type="float">Accelerometer X bias (m/s)</field>
+            <field name="ayBias" type="float">Accelerometer Y bias (m/s)</field>
+            <field name="azBias" type="float">Accelerometer Z bias (m/s)</field>
+            <field name="gxBias" type="float">Gyro X bias (rad/s)</field>
+            <field name="gyBias" type="float">Gyro Y bias (rad/s)</field>
+            <field name="gzBias" type="float">Gyro Z bias (rad/s)</field>
+        </message>
+        
+        <message name="DIAGNOSTIC" id="173">
+            Configurable diagnostic messages.
+            <field name="diagFl1" type="float">Diagnostic float 1</field>
+            <field name="diagFl2" type="float">Diagnostic float 2</field>
+            <field name="diagFl3" type="float">Diagnostic float 3</field>
+            <field name="diagSh1" type="int16_t">Diagnostic short 1</field>
+            <field name="diagSh2" type="int16_t">Diagnostic short 2</field>
+            <field name="diagSh3" type="int16_t">Diagnostic short 3</field>
+        </message>
+        
+        <message name="SLUGS_NAVIGATION" id="176">
+            Data used in the navigation algorithm.
+            <field name="u_m" type="float">Measured Airspeed prior to the Nav Filter</field>
+            <field name="phi_c" type="float">Commanded Roll</field>
+            <field name="theta_c" type="float">Commanded Pitch</field>
+            <field name="psiDot_c" type="float">Commanded Turn rate</field>
+            <field name="ay_body" type="float">Y component of the body acceleration</field>
+            <field name="totalDist" type="float">Total Distance to Run on this leg of Navigation</field>
+            <field name="dist2Go" type="float">Remaining distance to Run on this leg of Navigation</field>
+            <field name="fromWP" type="uint8_t">Origin WP</field>
+            <field name="toWP" type="uint8_t">Destination WP</field>
+        </message>
+        
+        <message name="DATA_LOG" id="177">
+            Configurable data log probes to be used inside Simulink
+            <field name="fl_1" type="float">Log value 1 </field>
+            <field name="fl_2" type="float">Log value 2 </field>
+            <field name="fl_3" type="float">Log value 3 </field>
+            <field name="fl_4" type="float">Log value 4 </field>
+            <field name="fl_5" type="float">Log value 5 </field>
+            <field name="fl_6" type="float">Log value 6 </field>
+        </message>
+        
+        <message name="GPS_DATE_TIME" id="179">
+            Pilot console PWM messges.
+            <field name="year"  type="uint8_t">Year reported by Gps </field>
+            <field name="month" type="uint8_t">Month reported by Gps </field>
+            <field name="day"   type="uint8_t">Day reported by Gps </field>
+            <field name="hour"  type="uint8_t">Hour reported by Gps </field>
+            <field name="min"   type="uint8_t">Min reported by Gps </field>
+            <field name="sec"   type="uint8_t">Sec reported by Gps  </field>
+            <field name="visSat"   type="uint8_t">Visible sattelites reported by Gps  </field>
+        </message>
+        
+        <message name="MID_LVL_CMDS" id="180">
+            Mid Level commands sent from the GS to the autopilot. These are only sent when being opperated in mid-level commands mode from the ground; for periodic report of these commands generated from the autopilot see message XXXX.
+            <field name="target" type="uint8_t">The system setting the commands</field>
+            <field name="hCommand" type="float">Commanded Airspeed</field>
+            <field name="uCommand" type="float">Log value 2 </field>
+            <field name="rCommand" type="float">Log value 3 </field>
+        </message>
+         
+        
+        <message name="CTRL_SRFC_PT" id="181">
+            This message configures the Selective Passthrough mode. it allows to select which control surfaces the Pilot can control from his console. It is implemented as a bitfield as follows: 
+            Position          Bit        Code
+            =================================
+            15-8             Reserved  
+            7                 dt_pass    128
+            6                 dla_pass   64
+            5                 dra_pass   32
+            4                 dr_pass    16
+            3                 dle_pass   8
+            2                 dre_pass   4
+            1                 dlf_pass   2
+            0                 drf_pass   1
+            Where Bit 15 is the MSb. 0 = AP has control of the surface; 1 = Pilot Console has control of the surface.
+            <field name="target" type="uint8_t">The system setting the commands</field>
+            <field name="bitfieldPt" type="uint16_t">Bitfield containing the PT configuration</field>
+        </message>
+         
+        
+        
+        <message name="SLUGS_ACTION" id="183">
+            Action messages focused on the SLUGS AP. 
+            <field name="target" type="uint8_t">The system reporting the action</field>
+            <field name="actionId" type="uint8_t">Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names</field>
+            <field name="actionVal" type="uint16_t">Value associated with the action</field>
+        </message>
+        
+    </messages>
+</mavlink>
diff --git a/thirdParty/mavlink/message_definitions/ualberta.xml b/thirdParty/mavlink/message_definitions/ualberta.xml
index eaa9d99844835df716665ef893d0bbcac5d47445..5e53e141e9f018e943abfd25bc1f58c0a0091574 100644
--- a/thirdParty/mavlink/message_definitions/ualberta.xml
+++ b/thirdParty/mavlink/message_definitions/ualberta.xml
@@ -1,54 +1,54 @@
 <?xml version='1.0'?>
 <mavlink>
-     <include>common.xml</include>
-     <enums>
-          <enum name="UALBERTA_AUTOPILOT_MODE">
-               <description>Available autopilot modes for ualberta uav</description>
-               <entry name="MODE_MANUAL_DIRECT">Raw input pulse widts sent to output</entry>
-               <entry name="MODE_MANUAL_SCALED">Inputs are normalized using calibration, the converted back to raw pulse widths for output</entry>
-               <entry name="MODE_AUTO_PID_ATT"> dfsdfs</entry>
-               <entry name="MODE_AUTO_PID_VEL"> dfsfds</entry>
-               <entry name="MODE_AUTO_PID_POS"> dfsdfsdfs</entry>
-          </enum>
-          <enum name="UALBERTA_NAV_MODE">
-               <description>Navigation filter mode</description>
-               <entry name="NAV_AHRS_INIT" />
-               <entry name="NAV_AHRS">AHRS mode</entry>
-               <entry name="NAV_INS_GPS_INIT">INS/GPS initialization mode</entry>
-               <entry name="NAV_INS_GPS">INS/GPS mode</entry>
-          </enum>
-          <enum name="UALBERTA_PILOT_MODE">
-               <description>Mode currently commanded by pilot</description>
-               <entry name="PILOT_MANUAL"> sdf</entry>
-               <entry name="PILOT_AUTO"> dfs</entry>
-               <entry name="PILOT_ROTO"> Rotomotion mode </entry>
-          </enum>
-     </enums>
-     <messages>
-          <message id="220" name="NAV_FILTER_BIAS">
-               <description>Accelerometer and Gyro biases from the navigation filter</description>
-               <field type="uint64_t" name="usec">Timestamp (microseconds)</field>
-               <field type="float" name="accel_0">b_f[0]</field>
-               <field type="float" name="accel_1">b_f[1]</field>
-               <field type="float" name="accel_2">b_f[2]</field>
-               <field type="float" name="gyro_0">b_f[0]</field>
-               <field type="float" name="gyro_1">b_f[1]</field>
-               <field type="float" name="gyro_2">b_f[2]</field>
-          </message>
-          <message id="221" name="RADIO_CALIBRATION">
-               <description>Complete set of calibration parameters for the radio</description>
-               <field type="uint16_t[3]" name="aileron">Aileron setpoints: left, center, right</field>
-               <field type="uint16_t[3]" name="elevator">Elevator setpoints: nose down, center, nose up</field>
-               <field type="uint16_t[3]" name="rudder">Rudder setpoints: nose left, center, nose right</field>
-               <field type="uint16_t[2]" name="gyro">Tail gyro mode/gain setpoints: heading hold, rate mode</field>
-               <field type="uint16_t[5]" name="pitch">Pitch curve setpoints (every 25%)</field>
-               <field type="uint16_t[5]" name="throttle">Throttle curve setpoints (every 25%)</field>
-          </message>
-          <message id="222" name="UALBERTA_SYS_STATUS">
-               <description>System status specific to ualberta uav</description>
-               <field type="uint8_t" name="mode">System mode, see UALBERTA_AUTOPILOT_MODE ENUM</field>
-               <field type="uint8_t" name="nav_mode">Navigation mode, see UALBERTA_NAV_MODE ENUM</field>
-               <field type="uint8_t" name="pilot">Pilot mode, see UALBERTA_PILOT_MODE</field>
-          </message>
-     </messages>
+    <include>common.xml</include>
+    <enums>
+        <enum name="UALBERTA_AUTOPILOT_MODE">
+            <description>Available autopilot modes for ualberta uav</description>
+            <entry name="MODE_MANUAL_DIRECT">Raw input pulse widts sent to output</entry>
+            <entry name="MODE_MANUAL_SCALED">Inputs are normalized using calibration, the converted back to raw pulse widths for output</entry>
+            <entry name="MODE_AUTO_PID_ATT"> dfsdfs</entry>
+            <entry name="MODE_AUTO_PID_VEL"> dfsfds</entry>
+            <entry name="MODE_AUTO_PID_POS"> dfsdfsdfs</entry>
+        </enum>
+        <enum name="UALBERTA_NAV_MODE">
+            <description>Navigation filter mode</description>
+            <entry name="NAV_AHRS_INIT" />
+            <entry name="NAV_AHRS">AHRS mode</entry>
+            <entry name="NAV_INS_GPS_INIT">INS/GPS initialization mode</entry>
+            <entry name="NAV_INS_GPS">INS/GPS mode</entry>
+        </enum>
+        <enum name="UALBERTA_PILOT_MODE">
+            <description>Mode currently commanded by pilot</description>
+            <entry name="PILOT_MANUAL"> sdf</entry>
+            <entry name="PILOT_AUTO"> dfs</entry>
+            <entry name="PILOT_ROTO"> Rotomotion mode </entry>
+        </enum>
+    </enums>
+    <messages>
+        <message id="220" name="NAV_FILTER_BIAS">
+            <description>Accelerometer and Gyro biases from the navigation filter</description>
+            <field type="uint64_t" name="usec">Timestamp (microseconds)</field>
+            <field type="float" name="accel_0">b_f[0]</field>
+            <field type="float" name="accel_1">b_f[1]</field>
+            <field type="float" name="accel_2">b_f[2]</field>
+            <field type="float" name="gyro_0">b_f[0]</field>
+            <field type="float" name="gyro_1">b_f[1]</field>
+            <field type="float" name="gyro_2">b_f[2]</field>
+        </message>
+        <message id="221" name="RADIO_CALIBRATION">
+            <description>Complete set of calibration parameters for the radio</description>
+            <field type="uint16_t[3]" name="aileron">Aileron setpoints: left, center, right</field>
+            <field type="uint16_t[3]" name="elevator">Elevator setpoints: nose down, center, nose up</field>
+            <field type="uint16_t[3]" name="rudder">Rudder setpoints: nose left, center, nose right</field>
+            <field type="uint16_t[2]" name="gyro">Tail gyro mode/gain setpoints: heading hold, rate mode</field>
+            <field type="uint16_t[5]" name="pitch">Pitch curve setpoints (every 25%)</field>
+            <field type="uint16_t[5]" name="throttle">Throttle curve setpoints (every 25%)</field>
+        </message>
+        <message id="222" name="UALBERTA_SYS_STATUS">
+            <description>System status specific to ualberta uav</description>
+            <field type="uint8_t" name="mode">System mode, see UALBERTA_AUTOPILOT_MODE ENUM</field>
+            <field type="uint8_t" name="nav_mode">Navigation mode, see UALBERTA_NAV_MODE ENUM</field>
+            <field type="uint8_t" name="pilot">Pilot mode, see UALBERTA_PILOT_MODE</field>
+        </message>
+    </messages>
 </mavlink>
diff --git a/thirdParty/mavlink/missionlib/testing/.gitignore b/thirdParty/mavlink/missionlib/testing/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..0776965a63b239ff52dbd7c2afe5aaa7db6c2262
--- /dev/null
+++ b/thirdParty/mavlink/missionlib/testing/.gitignore
@@ -0,0 +1,3 @@
+*.o
+udptest
+build
diff --git a/thirdParty/mavlink/missionlib/testing/main.c b/thirdParty/mavlink/missionlib/testing/main.c
new file mode 100644
index 0000000000000000000000000000000000000000..0bec1155ad5e73a5bdd765f71399e88bc0c59754
--- /dev/null
+++ b/thirdParty/mavlink/missionlib/testing/main.c
@@ -0,0 +1,276 @@
+/*******************************************************************************
+ 
+ Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch
+ and Bryan Godbolt godbolt ( a t ) ualberta.ca
+ 
+ adapted from example written by Bryan Godbolt godbolt ( a t ) ualberta.ca
+ 
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+ 
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+ 
+ You should have received a copy of the GNU General Public License
+ along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ 
+ ****************************************************************************/
+/*
+ This program sends some data to qgroundcontrol using the mavlink protocol.  The sent packets
+ cause qgroundcontrol to respond with heartbeats.  Any settings or custom commands sent from
+ qgroundcontrol are printed by this program along with the heartbeats.
+ 
+ 
+ I compiled this program sucessfully on Ubuntu 10.04 with the following command
+ 
+ gcc -I ../../pixhawk/mavlink/include -o udp-server udp.c
+ 
+ the rt library is needed for the clock_gettime on linux
+ */
+/* These headers are for QNX, but should all be standard on unix/linux */
+#include <stdio.h>
+#include <errno.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <netinet/in.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <time.h>
+#if (defined __QNX__) | (defined __QNXNTO__)
+/* QNX specific headers */
+#include <unix.h>
+#else
+/* Linux / MacOS POSIX timer headers */
+#include <sys/time.h>
+#include <time.h>
+#include <arpa/inet.h>
+#endif
+
+/* 0: Include MAVLink types */
+#include <../mavlink_types.h>
+
+/* 1: Define mavlink system storage */
+mavlink_system_t mavlink_system;
+
+/* 2: Include actual protocol, REQUIRES mavlink_system */
+#include <mavlink.h>
+
+/* 3: Define waypoint helper functions */
+void mavlink_wpm_send_message(mavlink_message_t* msg);
+void mavlink_wpm_send_gcs_string(const char* string);
+uint64_t mavlink_wpm_get_system_timestamp();
+
+/* 4: Include waypoint protocol */
+#include "waypoints.h"
+mavlink_wpm_storage wpm;
+
+
+#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why)
+
+char help[] = "--help";
+
+
+char target_ip[100];
+
+float position[6] = {};
+int sock;
+struct sockaddr_in gcAddr; 
+struct sockaddr_in locAddr;
+uint8_t buf[BUFFER_LENGTH];
+ssize_t recsize;
+socklen_t fromlen;
+int bytes_sent;
+mavlink_message_t msg;
+uint16_t len;
+int i = 0;
+unsigned int temp = 0;
+
+uint64_t microsSinceEpoch();
+
+
+
+
+/* Provide the interface functions for the waypoint manager */
+
+/*
+ *  @brief Sends a MAVLink message over UDP
+ */
+void mavlink_wpm_send_message(mavlink_message_t* msg)
+{
+	uint8_t buf[MAVLINK_MAX_PACKET_LEN];
+	uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
+	uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
+	
+	printf("SENT %d bytes", bytes_sent);
+}
+
+void mavlink_wpm_send_gcs_string(const char* string)
+{
+	printf("%s",string);
+}
+
+uint64_t mavlink_wpm_get_system_timestamp()
+{
+	struct timeval tv;
+	gettimeofday(&tv, NULL);
+	return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
+}
+
+
+
+int main(int argc, char* argv[])
+{	
+	// Initialize MAVLink
+	mavlink_wpm_init(&wpm);
+	mavlink_system.sysid = 1;
+	mavlink_system.compid = MAV_COMP_ID_WAYPOINTPLANNER;
+	
+	
+	
+	// Create socket
+	sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
+	
+	// Check if --help flag was used
+	if ((argc == 2) && (strcmp(argv[1], help) == 0))
+    {
+		printf("\n");
+		printf("\tUsage:\n\n");
+		printf("\t");
+		printf("%s", argv[0]);
+		printf(" <ip address of QGroundControl>\n");
+		printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
+		exit(EXIT_FAILURE);
+    }
+	
+	
+	// Change the target ip if parameter was given
+	strcpy(target_ip, "127.0.0.1");
+	if (argc == 2)
+    {
+		strcpy(target_ip, argv[1]);
+    }
+	
+	
+	memset(&locAddr, 0, sizeof(locAddr));
+	locAddr.sin_family = AF_INET;
+	locAddr.sin_addr.s_addr = INADDR_ANY;
+	locAddr.sin_port = htons(14551);
+	
+	/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ 
+	if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
+    {
+		perror("error bind failed");
+		close(sock);
+		exit(EXIT_FAILURE);
+    } 
+	
+	/* Attempt to make it non blocking */
+	if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
+    {
+		fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
+		close(sock);
+		exit(EXIT_FAILURE);
+    }
+	
+	
+	memset(&gcAddr, 0, sizeof(gcAddr));
+	gcAddr.sin_family = AF_INET;
+	gcAddr.sin_addr.s_addr = inet_addr(target_ip);
+	gcAddr.sin_port = htons(14550);
+	
+	
+	printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n");
+	
+	
+	for (;;) 
+    {
+		
+		/*Send Heartbeat */
+		mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_HELICOPTER, MAV_CLASS_GENERIC);
+		len = mavlink_msg_to_send_buffer(buf, &msg);
+		bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
+		
+		/* Send Status */
+		mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 0, 0);
+		len = mavlink_msg_to_send_buffer(buf, &msg);
+		bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
+		
+		/* Send Local Position */
+		mavlink_msg_local_position_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 
+										position[0], position[1], position[2],
+										position[3], position[4], position[5]);
+		len = mavlink_msg_to_send_buffer(buf, &msg);
+		bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
+		
+		/* Send attitude */
+		mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
+		len = mavlink_msg_to_send_buffer(buf, &msg);
+		bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
+		
+		
+		memset(buf, 0, BUFFER_LENGTH);
+		recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
+		if (recsize > 0)
+      	{
+			// Something received - print out all bytes and parse packet
+			mavlink_message_t msg;
+			mavlink_status_t status;
+			
+			printf("Bytes Received: %d\nDatagram: ", (int)recsize);
+			for (i = 0; i < recsize; ++i)
+			{
+				temp = buf[i];
+				printf("%02x ", (unsigned char)temp);
+				if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
+				{
+					// Packet received
+					printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
+					
+					// Handle packet with waypoint component
+					mavlink_wpm_message_handler(&msg);
+					
+					// Handle packet with parameter component
+				}
+			}
+			printf("\n");
+		}
+		memset(buf, 0, BUFFER_LENGTH);
+		usleep(50000); // Sleep one second
+    }
+}
+
+
+/* QNX timer version */
+#if (defined __QNX__) | (defined __QNXNTO__)
+uint64_t microsSinceEpoch()
+{
+	
+	struct timespec time;
+	
+	uint64_t micros = 0;
+	
+	clock_gettime(CLOCK_REALTIME, &time);  
+	micros = (uint64_t)time.tv_sec * 100000 + time.tv_nsec/1000;
+	
+	return micros;
+}
+#else
+uint64_t microsSinceEpoch()
+{
+	
+	struct timeval tv;
+	
+	uint64_t micros = 0;
+	
+	gettimeofday(&tv, NULL);  
+	micros =  ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
+	
+	return micros;
+}
+#endif
\ No newline at end of file
diff --git a/thirdParty/mavlink/missionlib/testing/udptest.xcodeproj/.gitignore b/thirdParty/mavlink/missionlib/testing/udptest.xcodeproj/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..d82a5e47def52cce2467649612ca36ad79f40a63
--- /dev/null
+++ b/thirdParty/mavlink/missionlib/testing/udptest.xcodeproj/.gitignore
@@ -0,0 +1,2 @@
+*.mode1v3
+*.pbxuser
diff --git a/thirdParty/mavlink/missionlib/testing/udptest.xcodeproj/project.pbxproj b/thirdParty/mavlink/missionlib/testing/udptest.xcodeproj/project.pbxproj
new file mode 100644
index 0000000000000000000000000000000000000000..ba27d2f32eed92413f59df2f43dc17a6830706f7
--- /dev/null
+++ b/thirdParty/mavlink/missionlib/testing/udptest.xcodeproj/project.pbxproj
@@ -0,0 +1,217 @@
+// !$*UTF8*$!
+{
+	archiveVersion = 1;
+	classes = {
+	};
+	objectVersion = 45;
+	objects = {
+
+/* Begin PBXBuildFile section */
+		34E8AFDB13F5064C001100AA /* waypoints.c in Sources */ = {isa = PBXBuildFile; fileRef = 34E8AFDA13F5064C001100AA /* waypoints.c */; };
+		8DD76FAC0486AB0100D96B5E /* main.c in Sources */ = {isa = PBXBuildFile; fileRef = 08FB7796FE84155DC02AAC07 /* main.c */; settings = {ATTRIBUTES = (); }; };
+		8DD76FB00486AB0100D96B5E /* udptest.1 in CopyFiles */ = {isa = PBXBuildFile; fileRef = C6A0FF2C0290799A04C91782 /* udptest.1 */; };
+/* End PBXBuildFile section */
+
+/* Begin PBXCopyFilesBuildPhase section */
+		8DD76FAF0486AB0100D96B5E /* CopyFiles */ = {
+			isa = PBXCopyFilesBuildPhase;
+			buildActionMask = 8;
+			dstPath = /usr/share/man/man1/;
+			dstSubfolderSpec = 0;
+			files = (
+				8DD76FB00486AB0100D96B5E /* udptest.1 in CopyFiles */,
+			);
+			runOnlyForDeploymentPostprocessing = 1;
+		};
+/* End PBXCopyFilesBuildPhase section */
+
+/* Begin PBXFileReference section */
+		08FB7796FE84155DC02AAC07 /* main.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = main.c; sourceTree = "<group>"; };
+		34E8AFDA13F5064C001100AA /* waypoints.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = waypoints.c; path = ../waypoints.c; sourceTree = SOURCE_ROOT; };
+		34E8AFDC13F50659001100AA /* waypoints.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = waypoints.h; path = ../waypoints.h; sourceTree = SOURCE_ROOT; };
+		8DD76FB20486AB0100D96B5E /* udptest */ = {isa = PBXFileReference; explicitFileType = "compiled.mach-o.executable"; includeInIndex = 0; path = udptest; sourceTree = BUILT_PRODUCTS_DIR; };
+		C6A0FF2C0290799A04C91782 /* udptest.1 */ = {isa = PBXFileReference; lastKnownFileType = text.man; path = udptest.1; sourceTree = "<group>"; };
+/* End PBXFileReference section */
+
+/* Begin PBXFrameworksBuildPhase section */
+		8DD76FAD0486AB0100D96B5E /* Frameworks */ = {
+			isa = PBXFrameworksBuildPhase;
+			buildActionMask = 2147483647;
+			files = (
+			);
+			runOnlyForDeploymentPostprocessing = 0;
+		};
+/* End PBXFrameworksBuildPhase section */
+
+/* Begin PBXGroup section */
+		08FB7794FE84155DC02AAC07 /* udptest */ = {
+			isa = PBXGroup;
+			children = (
+				34E8AFDC13F50659001100AA /* waypoints.h */,
+				08FB7795FE84155DC02AAC07 /* Source */,
+				C6A0FF2B0290797F04C91782 /* Documentation */,
+				1AB674ADFE9D54B511CA2CBB /* Products */,
+			);
+			name = udptest;
+			sourceTree = "<group>";
+		};
+		08FB7795FE84155DC02AAC07 /* Source */ = {
+			isa = PBXGroup;
+			children = (
+				34E8AFDA13F5064C001100AA /* waypoints.c */,
+				08FB7796FE84155DC02AAC07 /* main.c */,
+			);
+			name = Source;
+			sourceTree = "<group>";
+		};
+		1AB674ADFE9D54B511CA2CBB /* Products */ = {
+			isa = PBXGroup;
+			children = (
+				8DD76FB20486AB0100D96B5E /* udptest */,
+			);
+			name = Products;
+			sourceTree = "<group>";
+		};
+		C6A0FF2B0290797F04C91782 /* Documentation */ = {
+			isa = PBXGroup;
+			children = (
+				C6A0FF2C0290799A04C91782 /* udptest.1 */,
+			);
+			name = Documentation;
+			sourceTree = "<group>";
+		};
+/* End PBXGroup section */
+
+/* Begin PBXNativeTarget section */
+		8DD76FA90486AB0100D96B5E /* udptest */ = {
+			isa = PBXNativeTarget;
+			buildConfigurationList = 1DEB928508733DD80010E9CD /* Build configuration list for PBXNativeTarget "udptest" */;
+			buildPhases = (
+				8DD76FAB0486AB0100D96B5E /* Sources */,
+				8DD76FAD0486AB0100D96B5E /* Frameworks */,
+				8DD76FAF0486AB0100D96B5E /* CopyFiles */,
+			);
+			buildRules = (
+			);
+			dependencies = (
+			);
+			name = udptest;
+			productInstallPath = "$(HOME)/bin";
+			productName = udptest;
+			productReference = 8DD76FB20486AB0100D96B5E /* udptest */;
+			productType = "com.apple.product-type.tool";
+		};
+/* End PBXNativeTarget section */
+
+/* Begin PBXProject section */
+		08FB7793FE84155DC02AAC07 /* Project object */ = {
+			isa = PBXProject;
+			buildConfigurationList = 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "udptest" */;
+			compatibilityVersion = "Xcode 3.1";
+			developmentRegion = English;
+			hasScannedForEncodings = 1;
+			knownRegions = (
+				English,
+				Japanese,
+				French,
+				German,
+			);
+			mainGroup = 08FB7794FE84155DC02AAC07 /* udptest */;
+			projectDirPath = "";
+			projectRoot = "";
+			targets = (
+				8DD76FA90486AB0100D96B5E /* udptest */,
+			);
+		};
+/* End PBXProject section */
+
+/* Begin PBXSourcesBuildPhase section */
+		8DD76FAB0486AB0100D96B5E /* Sources */ = {
+			isa = PBXSourcesBuildPhase;
+			buildActionMask = 2147483647;
+			files = (
+				8DD76FAC0486AB0100D96B5E /* main.c in Sources */,
+				34E8AFDB13F5064C001100AA /* waypoints.c in Sources */,
+			);
+			runOnlyForDeploymentPostprocessing = 0;
+		};
+/* End PBXSourcesBuildPhase section */
+
+/* Begin XCBuildConfiguration section */
+		1DEB928608733DD80010E9CD /* Debug */ = {
+			isa = XCBuildConfiguration;
+			buildSettings = {
+				ALWAYS_SEARCH_USER_PATHS = NO;
+				COPY_PHASE_STRIP = NO;
+				GCC_DYNAMIC_NO_PIC = NO;
+				GCC_ENABLE_FIX_AND_CONTINUE = YES;
+				GCC_MODEL_TUNING = G5;
+				GCC_OPTIMIZATION_LEVEL = 0;
+				INSTALL_PATH = /usr/local/bin;
+				PRODUCT_NAME = udptest;
+			};
+			name = Debug;
+		};
+		1DEB928708733DD80010E9CD /* Release */ = {
+			isa = XCBuildConfiguration;
+			buildSettings = {
+				ALWAYS_SEARCH_USER_PATHS = NO;
+				DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym";
+				GCC_MODEL_TUNING = G5;
+				INSTALL_PATH = /usr/local/bin;
+				PRODUCT_NAME = udptest;
+			};
+			name = Release;
+		};
+		1DEB928A08733DD80010E9CD /* Debug */ = {
+			isa = XCBuildConfiguration;
+			buildSettings = {
+				ARCHS = "$(ARCHS_STANDARD_32_64_BIT)";
+				GCC_C_LANGUAGE_STANDARD = gnu99;
+				GCC_OPTIMIZATION_LEVEL = 0;
+				GCC_WARN_ABOUT_RETURN_TYPE = YES;
+				GCC_WARN_UNUSED_VARIABLE = YES;
+				HEADER_SEARCH_PATHS = ../../include/common/;
+				ONLY_ACTIVE_ARCH = YES;
+				PREBINDING = NO;
+				SDKROOT = macosx10.6;
+			};
+			name = Debug;
+		};
+		1DEB928B08733DD80010E9CD /* Release */ = {
+			isa = XCBuildConfiguration;
+			buildSettings = {
+				ARCHS = "$(ARCHS_STANDARD_32_64_BIT)";
+				GCC_C_LANGUAGE_STANDARD = gnu99;
+				GCC_WARN_ABOUT_RETURN_TYPE = YES;
+				GCC_WARN_UNUSED_VARIABLE = YES;
+				PREBINDING = NO;
+				SDKROOT = macosx10.6;
+			};
+			name = Release;
+		};
+/* End XCBuildConfiguration section */
+
+/* Begin XCConfigurationList section */
+		1DEB928508733DD80010E9CD /* Build configuration list for PBXNativeTarget "udptest" */ = {
+			isa = XCConfigurationList;
+			buildConfigurations = (
+				1DEB928608733DD80010E9CD /* Debug */,
+				1DEB928708733DD80010E9CD /* Release */,
+			);
+			defaultConfigurationIsVisible = 0;
+			defaultConfigurationName = Release;
+		};
+		1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "udptest" */ = {
+			isa = XCConfigurationList;
+			buildConfigurations = (
+				1DEB928A08733DD80010E9CD /* Debug */,
+				1DEB928B08733DD80010E9CD /* Release */,
+			);
+			defaultConfigurationIsVisible = 0;
+			defaultConfigurationName = Release;
+		};
+/* End XCConfigurationList section */
+	};
+	rootObject = 08FB7793FE84155DC02AAC07 /* Project object */;
+}
diff --git a/thirdParty/mavlink/missionlib/waypoints.c b/thirdParty/mavlink/missionlib/waypoints.c
new file mode 100644
index 0000000000000000000000000000000000000000..a7b8ee5f5ca0aa43be983ab1f1c1eaadaa616ee4
--- /dev/null
+++ b/thirdParty/mavlink/missionlib/waypoints.c
@@ -0,0 +1,1044 @@
+/*******************************************************************************
+ 
+ Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch
+ 
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+ 
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+ 
+ You should have received a copy of the GNU General Public License
+ along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ 
+ ****************************************************************************/
+
+#include "waypoints.h"
+#include <math.h>
+
+bool debug = true;
+bool verbose = true;
+
+extern mavlink_system_t mavlink_system;
+extern mavlink_wpm_storage wpm;
+
+extern void mavlink_wpm_send_message(mavlink_message_t* msg);
+extern void mavlink_wpm_send_gcs_string(const char* string);
+extern uint64_t mavlink_wpm_get_system_timestamp();
+
+
+#define MAVLINK_WPM_NO_PRINTF
+
+void mavlink_wpm_init(mavlink_wpm_storage* state)
+{
+	// Set all waypoints to zero
+	
+	// Set count to zero
+	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->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT;
+	state->idle = false;      				///< indicates if the system is following the waypoints or is waiting
+	state->current_active_wp_id = -1;		///< id of current waypoint
+	state->yaw_reached = false;						///< boolean for yaw attitude reached
+	state->pos_reached = false;						///< boolean for position reached
+	state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
+	state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
+	
+}
+
+/*
+ *  @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_waypoint_ack_t wpa;
+	
+    wpa.target_system = wpm.current_partner_sysid;
+    wpa.target_component = wpm.current_partner_compid;
+    wpa.type = type;
+	
+    mavlink_msg_waypoint_ack_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpa);
+    mavlink_wpm_send_message(&msg);
+	
+    // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
+	
+    if (MAVLINK_WPM_TEXT_FEEDBACK)
+	{
+#ifdef MAVLINK_WPM_NO_PRINTF
+    	mavlink_wpm_send_gcs_string("Sent waypoint ACK");
+#else
+		if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
+#endif
+		mavlink_wpm_send_gcs_string("Sent waypoint ACK");
+	}
+}
+
+/*
+ *  @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_waypoint_t *cur = &(wpm.waypoints[seq]);
+		
+        mavlink_message_t msg;
+        mavlink_waypoint_current_t wpc;
+		
+        wpc.seq = cur->seq;
+		
+        mavlink_msg_waypoint_current_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc);
+        mavlink_wpm_send_message(&msg);
+		
+        // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
+		
+        if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Broadcasted new current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq);
+    }
+    else
+    {
+        if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: index out of bounds\n");
+    }
+}
+
+/*
+ *  @brief Directs the MAV to fly to a position
+ *
+ *  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_setpoint(uint16_t seq)
+{
+    if(seq < wpm.size)
+    {
+        mavlink_waypoint_t *cur = &(wpm.waypoints[seq]);
+		
+        mavlink_message_t msg;
+        mavlink_local_position_setpoint_set_t position_control_set_point;
+		
+        // Send new NED or ENU setpoint to onbaord autopilot
+        if (cur->frame == MAV_FRAME_LOCAL_NED || cur->frame == MAV_FRAME_LOCAL_ENU)
+        {
+            position_control_set_point.target_system = mavlink_system.sysid;
+            position_control_set_point.target_component = MAV_COMP_ID_IMU;
+            position_control_set_point.x = cur->x;
+            position_control_set_point.y = cur->y;
+            position_control_set_point.z = cur->z;
+            position_control_set_point.yaw = cur->param4;
+			
+            mavlink_msg_local_position_setpoint_set_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &position_control_set_point);
+            mavlink_wpm_send_message(&msg);
+			
+            // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
+        }
+        else
+        {
+            if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//// if (verbose) // printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq);
+        }
+		
+        wpm.timestamp_last_send_setpoint = mavlink_wpm_get_system_timestamp();
+    }
+    else
+    {
+        if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n");
+    }
+}
+
+void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count)
+{
+    mavlink_message_t msg;
+    mavlink_waypoint_count_t wpc;
+	
+    wpc.target_system = wpm.current_partner_sysid;
+    wpc.target_component = wpm.current_partner_compid;
+    wpc.count = count;
+	
+    mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc);
+    mavlink_wpm_send_message(&msg);
+	
+    if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system);
+	
+    // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
+}
+
+void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
+{
+    if (seq < wpm.size)
+    {
+        mavlink_message_t msg;
+        mavlink_waypoint_t *wp = &(wpm.waypoints[seq]);
+        wp->target_system = wpm.current_partner_sysid;
+        wp->target_component = wpm.current_partner_compid;
+        mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_system.compid, &msg, wp);
+        mavlink_wpm_send_message(&msg);
+        if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
+		
+        // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
+    }
+    else
+    {
+        if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n");
+    }
+}
+
+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_waypoint_request_t wpr;
+        wpr.target_system = wpm.current_partner_sysid;
+        wpr.target_component = wpm.current_partner_compid;
+        wpr.seq = seq;
+        mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpr);
+        mavlink_wpm_send_message(&msg);
+        if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system);
+		
+        // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
+    }
+    else
+    {
+        if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n");
+    }
+}
+
+/*
+ *  @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_waypoint_reached_t wp_reached;
+	
+    wp_reached.seq = seq;
+	
+    mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wp_reached);
+    mavlink_wpm_send_message(&msg);
+	
+    if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq);
+	
+    // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
+}
+
+//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z)
+//{
+//    if (seq < wpm.size)
+//    {
+//        mavlink_waypoint_t *cur = waypoints->at(seq);
+//		
+//        const PxVector3 A(cur->x, cur->y, cur->z);
+//        const PxVector3 C(x, y, z);
+//		
+//        // seq not the second last waypoint
+//        if ((uint16_t)(seq+1) < wpm.size)
+//        {
+//            mavlink_waypoint_t *next = waypoints->at(seq+1);
+//            const PxVector3 B(next->x, next->y, next->z);
+//            const float r = (B-A).dot(C-A) / (B-A).lengthSquared();
+//            if (r >= 0 && r <= 1)
+//            {
+//                const PxVector3 P(A + r*(B-A));
+//                return (P-C).length();
+//            }
+//            else if (r < 0.f)
+//            {
+//                return (C-A).length();
+//            }
+//            else
+//            {
+//                return (C-B).length();
+//            }
+//        }
+//        else
+//        {
+//            return (C-A).length();
+//        }
+//    }
+//    else
+//    {
+//        // if (verbose) // printf("ERROR: index out of bounds\n");
+//    }
+//    return -1.f;
+//}
+
+float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z)
+{
+	//    if (seq < wpm.size)
+	//    {
+	//        mavlink_waypoint_t *cur = waypoints->at(seq);
+	//		
+	//        const PxVector3 A(cur->x, cur->y, cur->z);
+	//        const PxVector3 C(x, y, z);
+	//		
+	//        return (C-A).length();
+	//    }
+	//    else
+	//    {
+	//        // if (verbose) // printf("ERROR: index out of bounds\n");
+	//    }
+    return -1.f;
+}
+
+
+void mavlink_wpm_message_handler(const mavlink_message_t* msg)
+{
+    // Handle param messages
+    //paramClient->handleMAVLinkPacket(msg);
+	
+    //check for timed-out operations
+    uint64_t now = mavlink_wpm_get_system_timestamp();
+    if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
+    {
+#ifdef MAVLINK_WPM_NO_PRINTF
+    	mavlink_wpm_send_gcs_string("Operation timeout switching -> IDLE");
+#else
+		if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state);
+#endif
+		wpm.current_state = MAVLINK_WPM_STATE_IDLE;
+        wpm.current_count = 0;
+        wpm.current_partner_sysid = 0;
+        wpm.current_partner_compid = 0;
+        wpm.current_wp_id = -1;
+		
+        if(wpm.size == 0)
+        {
+            wpm.current_active_wp_id = -1;
+        }
+    }
+	
+    if(now-wpm.timestamp_last_send_setpoint > wpm.delay_setpoint && wpm.current_active_wp_id < wpm.size)
+    {
+        mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
+    }
+	
+    switch(msg->msgid)
+    {
+		case MAVLINK_MSG_ID_ATTITUDE:
+        {
+            if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size)
+            {
+                mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]);
+                if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED)
+                {
+                    mavlink_attitude_t att;
+                    mavlink_msg_attitude_decode(msg, &att);
+                    float yaw_tolerance = wpm.accept_range_yaw;
+                    //compare current yaw
+                    if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI)
+                    {
+                        if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
+                            wpm.yaw_reached = true;
+                    }
+                    else if(att.yaw - yaw_tolerance < 0.0f)
+                    {
+                        float lowerBound = 360.0f + att.yaw - yaw_tolerance;
+                        if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
+                            wpm.yaw_reached = true;
+                    }
+                    else
+                    {
+                        float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI;
+                        if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
+                            wpm.yaw_reached = true;
+                    }
+                }
+            }
+            break;
+        }
+			
+		case MAVLINK_MSG_ID_LOCAL_POSITION:
+        {
+            if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size)
+            {
+                mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]);
+				
+                if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED)
+                {
+                    mavlink_local_position_t pos;
+                    mavlink_msg_local_position_decode(msg, &pos);
+                    //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
+					
+                    wpm.pos_reached = false;
+					
+                    // compare current position (given in message) with current waypoint
+                    float orbit = wp->param1;
+					
+                    float dist;
+                    if (wp->param2 == 0)
+                    {
+						// FIXME segment distance
+                        //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z);
+                    }
+                    else
+                    {
+                        dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z);
+                    }
+					
+                    if (dist >= 0.f && dist <= orbit && wpm.yaw_reached)
+                    {
+                        wpm.pos_reached = true;
+                    }
+                }
+            }
+            break;
+        }
+			
+			//		case MAVLINK_MSG_ID_CMD: // special action from ground station
+			//        {
+			//            mavlink_cmd_t action;
+			//            mavlink_msg_cmd_decode(msg, &action);
+			//            if(action.target == mavlink_system.sysid)
+			//            {
+			//                // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
+			//                switch (action.action)
+			//                {
+			//						//				case MAV_ACTION_LAUNCH:
+			//						//					// if (verbose) std::cerr << "Launch received" << std::endl;
+			//						//					current_active_wp_id = 0;
+			//						//					if (wpm.size>0)
+			//						//					{
+			//						//						setActive(waypoints[current_active_wp_id]);
+			//						//					}
+			//						//					else
+			//						//						// if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
+			//						//					break;
+			//						
+			//						//				case MAV_ACTION_CONTINUE:
+			//						//					// if (verbose) std::c
+			//						//					err << "Continue received" << std::endl;
+			//						//					idle = false;
+			//						//					setActive(waypoints[current_active_wp_id]);
+			//						//					break;
+			//						
+			//						//				case MAV_ACTION_HALT:
+			//						//					// if (verbose) std::cerr << "Halt received" << std::endl;
+			//						//					idle = true;
+			//						//					break;
+			//						
+			//						//				default:
+			//						//					// if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
+			//						//					break;
+			//                }
+			//            }
+			//            break;
+			//        }
+			
+		case MAVLINK_MSG_ID_WAYPOINT_ACK:
+        {
+            mavlink_waypoint_ack_t wpa;
+            mavlink_msg_waypoint_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_system.compid*/))
+            {
+                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)
+                    {
+#ifdef MAVLINK_WPM_NO_PRINTF
+						mavlink_wpm_send_gcs_string("Got last WP ACK state -> IDLE");
+#else
+						if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
+#endif
+						wpm.current_state = MAVLINK_WPM_STATE_IDLE;
+                        wpm.current_wp_id = 0;
+                    }
+                }
+            }
+			else
+			{
+#ifdef MAVLINK_WPM_NO_PRINTF
+				mavlink_wpm_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
+#else
+				if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
+#endif
+			}
+            break;
+        }
+			
+		case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
+        {
+            mavlink_waypoint_set_current_t wpc;
+            mavlink_msg_waypoint_set_current_decode(msg, &wpc);
+			
+            if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_system.compid*/)
+            {
+                wpm.timestamp_lastaction = now;
+				
+                if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
+                {
+                    if (wpc.seq < wpm.size)
+                    {
+                        // if (verbose) // printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n");
+                        wpm.current_active_wp_id = wpc.seq;
+                        uint32_t i;
+                        for(i = 0; i < wpm.size; i++)
+                        {
+                            if (i == wpm.current_active_wp_id)
+                            {
+                                wpm.waypoints[i].current = true;
+                            }
+                            else
+                            {
+                                wpm.waypoints[i].current = false;
+                            }
+                        }
+#ifdef MAVLINK_WPM_NO_PRINTF
+						mavlink_wpm_send_gcs_string("NEW WP SET");
+#else
+						if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm.current_active_wp_id);
+#endif
+                        wpm.yaw_reached = false;
+                        wpm.pos_reached = false;
+                        mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
+                        mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
+                        wpm.timestamp_firstinside_orbit = 0;
+                    }
+                    else
+                    {
+#ifdef MAVLINK_WPM_NO_PRINTF
+						mavlink_wpm_send_gcs_string("IGN WP CURR CMD: Not in list");
+#else
+						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
+#endif
+                    }
+                }
+				else
+				{
+#ifdef MAVLINK_WPM_NO_PRINTF
+					mavlink_wpm_send_gcs_string("IGN WP CURR CMD: Busy");
+#else
+					if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
+#endif
+				}
+            }
+			else
+			{
+#ifdef MAVLINK_WPM_NO_PRINTF
+				mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch");
+#else
+				if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
+#endif
+			}
+            break;
+        }
+			
+		case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
+        {
+            mavlink_waypoint_request_list_t wprl;
+            mavlink_msg_waypoint_request_list_decode(msg, &wprl);
+            if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_system.compid*/)
+            {
+                wpm.timestamp_lastaction = now;
+				
+                if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
+                {
+                    if (wpm.size > 0)
+                    {
+                        if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
+							if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
+								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) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
+                    }
+                    wpm.current_count = wpm.size;
+                    mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count);
+                }
+                else
+                {
+                    // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state);
+                }
+            }
+			else
+			{
+				// if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n");
+			}
+			
+            break;
+        }
+			
+		case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
+        {
+            mavlink_waypoint_request_t wpr;
+            mavlink_msg_waypoint_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_system.compid*/)
+            {
+                wpm.timestamp_lastaction = now;
+				
+                //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 && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size))
+                {
+                    if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
+                    {
+#ifdef MAVLINK_WPM_NO_PRINTF
+						mavlink_wpm_send_gcs_string("GOT WP REQ, state -> SEND");
+#else
+						if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+#endif
+                    }
+                    if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1)
+                    {
+#ifdef MAVLINK_WPM_NO_PRINTF
+						mavlink_wpm_send_gcs_string("GOT 2nd WP REQ");
+#else
+						if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+#endif
+                    }
+                    if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id)
+                    {
+#ifdef MAVLINK_WPM_NO_PRINTF
+						mavlink_wpm_send_gcs_string("GOT 2nd WP REQ");
+#else
+						if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
+#endif
+                    }
+					
+                    wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
+                    wpm.current_wp_id = wpr.seq;
+                    mavlink_wpm_send_waypoint(wpm.current_partner_sysid, wpm.current_partner_compid, wpr.seq);
+                }
+                else
+                {
+                    // if (verbose)
+                    {
+                        if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS))
+						{
+#ifdef MAVLINK_WPM_NO_PRINTF
+							mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy");
+#else
+							if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state);
+#endif
+							break;
+						}
+                        else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
+                        {
+                            if (wpr.seq != 0)
+							{
+#ifdef MAVLINK_WPM_NO_PRINTF
+								mavlink_wpm_send_gcs_string("REJ. WP CMD: First id != 0");
+#else
+								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
+#endif
+							}
+                        }
+                        else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)
+                        {
+                            if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1)
+							{
+#ifdef MAVLINK_WPM_NO_PRINTF
+								mavlink_wpm_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
+#else
+								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1);
+#endif
+							}
+							else if (wpr.seq >= wpm.size)
+							{
+#ifdef MAVLINK_WPM_NO_PRINTF
+								mavlink_wpm_send_gcs_string("REJ. WP CMD: Req. WP not in list");
+#else
+								if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
+#endif
+							}
+                        }
+                        else
+						{
+#ifdef MAVLINK_WPM_NO_PRINTF
+							mavlink_wpm_send_gcs_string("REJ. WP CMD: ?");
+#else
+							if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n");
+#endif
+						}
+                    }
+                }
+            }
+            else
+            {
+                //we we're target but already communicating with someone else
+                if((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid))
+                {
+#ifdef MAVLINK_WPM_NO_PRINTF
+					mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy");
+#else
+					if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid);
+#endif
+                }
+				else
+				{
+#ifdef MAVLINK_WPM_NO_PRINTF
+					mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch");
+#else
+					if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
+#endif
+				}
+				
+            }
+            break;
+        }
+			
+		case MAVLINK_MSG_ID_WAYPOINT_COUNT:
+        {
+            mavlink_waypoint_count_t wpc;
+            mavlink_msg_waypoint_count_decode(msg, &wpc);
+            if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_system.compid*/)
+            {
+                wpm.timestamp_lastaction = now;
+				
+                if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0))
+                {
+                    if (wpc.count > 0)
+                    {
+                        if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
+                        {
+#ifdef MAVLINK_WPM_NO_PRINTF
+							mavlink_wpm_send_gcs_string("WP CMD OK: state -> GETLIST");
+#else
+							if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
+#endif
+                        }
+                        if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST)
+                        {
+#ifdef MAVLINK_WPM_NO_PRINTF
+							mavlink_wpm_send_gcs_string("WP CMD OK AGAIN");
+#else
+							if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
+#endif
+                        }
+						
+                        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;
+						
+#ifdef MAVLINK_WPM_NO_PRINTF
+						mavlink_wpm_send_gcs_string("CLR RCV BUF: READY");
+#else
+						if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n");
+#endif
+						wpm.rcv_size = 0;
+                        //while(waypoints_receive_buffer->size() > 0)
+						//                        {
+						//                            delete waypoints_receive_buffer->back();
+						//                            waypoints_receive_buffer->pop_back();
+						//                        }
+						
+                        mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id);
+                    }
+                    else if (wpc.count == 0)
+                    {
+#ifdef MAVLINK_WPM_NO_PRINTF
+						mavlink_wpm_send_gcs_string("COUNT 0");
+#else
+						if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n");
+#endif
+						wpm.rcv_size = 0;
+                        //while(waypoints_receive_buffer->size() > 0)
+						//                        {
+						//                            delete waypoints->back();
+						//                            waypoints->pop_back();
+						//                        }
+                        wpm.current_active_wp_id = -1;
+                        wpm.yaw_reached = false;
+                        wpm.pos_reached = false;
+                        break;
+						
+                    }
+                    else
+                    {
+#ifdef MAVLINK_WPM_NO_PRINTF
+						mavlink_wpm_send_gcs_string("IGN WP CMD");
+#else
+						if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
+#endif
+                    }
+                }
+                else
+                {
+                    if (!(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST))
+					{
+#ifdef MAVLINK_WPM_NO_PRINTF
+						mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy");
+#else
+						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state);
+#endif
+					}
+                    else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0)
+					{
+#ifdef MAVLINK_WPM_NO_PRINTF
+						mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy");
+#else
+						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id);
+#endif
+					}
+                    else
+					{
+#ifdef MAVLINK_WPM_NO_PRINTF
+						mavlink_wpm_send_gcs_string("REJ. WP CMD: ?");
+#else
+						if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
+#endif
+					}
+                }
+            }
+			else
+			{
+#ifdef MAVLINK_WPM_NO_PRINTF
+				mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch");
+#else
+				if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
+#endif
+			}
+            
+        }
+			break;
+			
+		case MAVLINK_MSG_ID_WAYPOINT:
+        {
+            mavlink_waypoint_t wp;
+            mavlink_msg_waypoint_decode(msg, &wp);
+			
+			// if (verbose) // printf("GOT WAYPOINT!");
+			
+            if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/))
+            {
+                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 && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count))
+                {
+                    if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
+						if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid);
+							if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid);
+								
+								wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
+                    mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]);
+                    memcpy(newwp, &wp, sizeof(mavlink_waypoint_t));
+					
+					wpm.current_wp_id = wp.seq + 1;
+					
+                    // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
+					
+                    if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)
+                    {
+                        // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count);
+						
+                        mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
+						
+                        if (wpm.current_active_wp_id > wpm.rcv_size-1)
+                        {
+                            wpm.current_active_wp_id = wpm.rcv_size-1;
+                        }
+						
+                        // switch the waypoints list
+						// FIXME CHECK!!!
+						for (int i = 0; i < wpm.current_count; ++i)
+						{
+							wpm.waypoints[i] = wpm.rcv_waypoints[i];
+						}
+						wpm.size = wpm.current_count;
+						
+                        //get the new current waypoint
+                        uint32_t i;
+                        for(i = 0; i < wpm.size; i++)
+                        {
+                            if (wpm.waypoints[i].current == 1)
+                            {
+                                wpm.current_active_wp_id = i;
+                                //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id);
+                                wpm.yaw_reached = false;
+                                wpm.pos_reached = false;
+								mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
+                                mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
+								wpm.timestamp_firstinside_orbit = 0;
+                                break;
+                            }
+                        }
+						
+                        if (i == wpm.size)
+                        {
+                            wpm.current_active_wp_id = -1;
+                            wpm.yaw_reached = false;
+                            wpm.pos_reached = false;
+                            wpm.timestamp_firstinside_orbit = 0;
+                        }
+						
+                        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
+                {
+                    if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
+                    {
+                        //we're done receiving waypoints, answer with ack.
+                        mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
+                        // printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
+                    }
+                    // if (verbose)
+                    {
+                        if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS))
+						{
+							// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state);
+							break;
+						}
+                        else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST)
+                        {
+                            if(!(wp.seq == 0))
+							{
+								// printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq);
+							}
+                            else
+							{
+								// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
+							}
+                        }
+                        else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)
+                        {
+                            if (!(wp.seq == wpm.current_wp_id))
+							{
+								// printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id);
+							}
+                            else if (!(wp.seq < wpm.current_count))
+							{
+								// printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq);
+							}
+                            else
+							{
+								// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
+							}
+                        }
+                        else
+						{
+							// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
+						}
+                    }
+                }
+            }
+            else
+            {
+                //we we're target but already communicating with someone else
+                if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
+                {
+                    // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid);
+                }
+                else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_system.compid*/)
+                {
+                    // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
+                }
+            }
+            break;
+        }
+			
+		case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
+        {
+            mavlink_waypoint_clear_all_t wpca;
+            mavlink_msg_waypoint_clear_all_decode(msg, &wpca);
+			
+            if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE)
+            {
+                wpm.timestamp_lastaction = now;
+				
+                // if (verbose) // printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
+                // Delete all waypoints
+				wpm.size = 0;
+                wpm.current_active_wp_id = -1;
+                wpm.yaw_reached = false;
+                wpm.pos_reached = false;
+            }
+            else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
+            {
+                // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state);
+            }
+            break;
+        }
+			
+		default:
+        {
+            // if (debug) // printf("Waypoint: received message of unknown type");
+            break;
+        }
+    }
+	
+    //check if the current waypoint was reached
+    if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle)
+    {
+        if (wpm.current_active_wp_id < wpm.size)
+        {
+            mavlink_waypoint_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]);
+			
+            if (wpm.timestamp_firstinside_orbit == 0)
+            {
+                // Announce that last waypoint was reached
+                // if (verbose) // printf("*** Reached waypoint %u ***\n", cur_wp->seq);
+                mavlink_wpm_send_waypoint_reached(cur_wp->seq);
+                wpm.timestamp_firstinside_orbit = now;
+            }
+			
+            // check if the MAV was long enough inside the waypoint orbit
+            //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
+            if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000)
+            {
+                if (cur_wp->autocontinue)
+                {
+                    cur_wp->current = 0;
+                    if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1)
+                    {
+                        //the last waypoint was reached, if auto continue is
+                        //activated restart the waypoint list from the beginning
+                        wpm.current_active_wp_id = 1;
+                    }
+                    else
+                    {
+                        if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size)
+                            wpm.current_active_wp_id++;
+                    }
+					
+                    // Fly to next waypoint
+                    wpm.timestamp_firstinside_orbit = 0;
+                    mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
+                    mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
+                    wpm.waypoints[wpm.current_active_wp_id].current = true;
+                    wpm.pos_reached = false;
+                    wpm.yaw_reached = false;
+                    // if (verbose) // printf("Set new waypoint (%u)\n", wpm.current_active_wp_id);
+                }
+            }
+        }
+    }
+    else
+    {
+        wpm.timestamp_lastoutside_orbit = now;
+    }
+}
+
diff --git a/thirdParty/mavlink/missionlib/waypoints.h b/thirdParty/mavlink/missionlib/waypoints.h
new file mode 100644
index 0000000000000000000000000000000000000000..4bc32a14a4368cf68e2dd69b60b027c8a93f4e65
--- /dev/null
+++ b/thirdParty/mavlink/missionlib/waypoints.h
@@ -0,0 +1,91 @@
+/*******************************************************************************
+ 
+ Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch
+ 
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+ 
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+ 
+ You should have received a copy of the GNU General Public License
+ along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ 
+ ****************************************************************************/
+
+/* This assumes you have the mavlink headers on your include path
+ or in the same folder as this source file */
+#include <mavlink.h>
+#include <stdbool.h>
+
+// FIXME XXX - TO BE MOVED TO XML
+enum MAVLINK_WPM_STATES
+{
+    MAVLINK_WPM_STATE_IDLE = 0,
+    MAVLINK_WPM_STATE_SENDLIST,
+    MAVLINK_WPM_STATE_SENDLIST_SENDWPS,
+    MAVLINK_WPM_STATE_GETLIST,
+    MAVLINK_WPM_STATE_GETLIST_GETWPS,
+    MAVLINK_WPM_STATE_GETLIST_GOTALL,
+	MAVLINK_WPM_STATE_ENUM_END
+};
+
+enum MAVLINK_WPM_CODES
+{
+    MAVLINK_WPM_CODE_OK = 0,
+    MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
+    MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
+    MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
+    MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
+    MAVLINK_WPM_CODE_ENUM_END
+};
+
+
+/* WAYPOINT MANAGER - MISSION LIB */
+
+#define MAVLINK_WPM_MAX_WP_COUNT 100
+#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE				  ///< Enable double buffer and in-flight updates
+#define MAVLINK_WPM_TEXT_FEEDBACK 1						  ///< Report back status information as text
+#define MAVLINK_WPM_SYSTEM_ID 1
+#define MAVLINK_WPM_COMPONENT_ID 1
+#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 2000000
+#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000
+#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40
+
+
+struct mavlink_wpm_storage {
+	mavlink_waypoint_t waypoints[MAVLINK_WPM_MAX_WP_COUNT];      ///< Currently active waypoints
+#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
+	mavlink_waypoint_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT];  ///< Receive buffer for next waypoints
+#endif
+	uint16_t size;
+	uint16_t max_size;
+	uint16_t rcv_size;
+	enum MAVLINK_WPM_STATES current_state;
+	uint16_t current_wp_id;							///< Waypoint in current transmission
+	uint16_t current_active_wp_id;					///< Waypoint the system is currently heading towards
+	uint16_t current_count;
+	uint8_t current_partner_sysid;
+	uint8_t current_partner_compid;
+	uint64_t timestamp_lastaction;
+	uint64_t timestamp_last_send_setpoint;
+	uint64_t timestamp_firstinside_orbit;
+	uint64_t timestamp_lastoutside_orbit;
+	uint32_t timeout;
+	uint32_t delay_setpoint;
+	float accept_range_yaw;
+	float accept_range_distance;
+	bool yaw_reached;
+	bool pos_reached;
+	bool idle;
+};
+
+typedef struct mavlink_wpm_storage mavlink_wpm_storage;
+
+void mavlink_wpm_init(mavlink_wpm_storage* state);
+
+void mavlink_wpm_message_handler(const mavlink_message_t* msg);
\ No newline at end of file