mavlink: minor refactoring and cleanup, rate limiter class implemented, new messages added

9 files changed, 371 insertions, 221 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 3102c937d..8a026742c 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -79,6 +79,7 @@
 #include "mavlink_main.h"
 #include "mavlink_messages.h"
 #include "mavlink_receiver.h"
+#include "mavlink_rate_limiter.h"
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -321,7 +322,7 @@ Mavlink::get_uart_fd()
 mavlink_channel_t
 Mavlink::get_channel()
 {
-	return _chan;
+	return _channel;
 }
 
 /****************************************************************************
@@ -1321,7 +1322,7 @@ Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
 {
 	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
 
-	mavlink_send_uart_bytes(_chan, missionlib_msg_buf, len);
+	mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
 }
 
 
@@ -1356,7 +1357,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
 	}
 }
 
-MavlinkOrbSubscription *Mavlink::add_orb_subscription(const struct orb_metadata *topic, const size_t size)
+MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, const size_t size)
 {
 	/* check if already subscribed to this topic */
 	MavlinkOrbSubscription *sub;
@@ -1406,7 +1407,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 	int ch;
 	_baudrate = 57600;
-	_chan = MAVLINK_COMM_0;
+	_channel = MAVLINK_COMM_0;
 
 	_mode = MODE_OFFBOARD;
 
@@ -1520,8 +1521,6 @@ Mavlink::task_main(int argc, char *argv[])
 
 	thread_running = true;
 
-	unsigned lowspeed_counter = 0;
-
 	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update), sizeof(parameter_update_s));
 	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status), sizeof(vehicle_status_s));
 
@@ -1530,20 +1529,42 @@ Mavlink::task_main(int argc, char *argv[])
 	warnx("started");
 	mavlink_log_info(_mavlink_fd, "[mavlink] started");
 
-	/* add default streams, intervals depend on baud rate */
+	/* add default streams depending on mode, intervals depend on baud rate */
 	float rate_mult = _baudrate / 57600.0f;
 	if (rate_mult > 4.0f) {
 		rate_mult = 4.0f;
 	}
 
 	add_stream("HEARTBEAT", 1.0f);
-	add_stream("SYS_STATUS", 1.0f * rate_mult);
-	add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
-	add_stream("HIGHRES_IMU", 1.0f * rate_mult);
-	add_stream("ATTITUDE", 10.0f * rate_mult);
-	add_stream("GPS_RAW_INT", 1.0f * rate_mult);
-	add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult);
-	add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult);
+
+	switch(_mode) {
+	case MODE_OFFBOARD:
+		add_stream("SYS_STATUS", 1.0f * rate_mult);
+		add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
+		add_stream("HIGHRES_IMU", 1.0f * rate_mult);
+		add_stream("ATTITUDE", 10.0f * rate_mult);
+		add_stream("GPS_RAW_INT", 1.0f * rate_mult);
+		add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult);
+		add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult);
+		add_stream("SERVO_OUTPUT_RAW_0", 1.0f * rate_mult);
+		break;
+
+	case MODE_HIL:
+		add_stream("SYS_STATUS", 1.0f * rate_mult);
+		add_stream("GPS_GLOBAL_ORIGIN", 0.5f * rate_mult);
+		add_stream("HIGHRES_IMU", 1.0f * rate_mult);
+		add_stream("ATTITUDE", 10.0f * rate_mult);
+		add_stream("GPS_RAW_INT", 1.0f * rate_mult);
+		add_stream("GLOBAL_POSITION_INT", 5.0f * rate_mult);
+		add_stream("LOCAL_POSITION_NED", 5.0f * rate_mult);
+		break;
+
+	default:
+		break;
+	}
+
+	MavlinkRateLimiter slow_rate_limiter(2000.0f / rate_mult);
+	MavlinkRateLimiter fast_rate_limiter(100.0f / rate_mult);
 
 	while (!_task_should_exit) {
 		/* main loop */
@@ -1571,13 +1592,6 @@ Mavlink::task_main(int argc, char *argv[])
 			stream->update(t);
 		}
 
-		/* 0.5 Hz */
-		if (lowspeed_counter % (2000000 / MAIN_LOOP_DELAY) == 0) {
-			mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
-		}
-
-		lowspeed_counter++;
-
 		bool updated;
 		orb_check(mission_result_sub, &updated);
 
@@ -1591,18 +1605,16 @@ Mavlink::task_main(int argc, char *argv[])
 			}
 
 			mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
+		} else {
+			if (slow_rate_limiter.check(t)) {
+				mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
+			}
 		}
 
-		if (lowspeed_counter % (50000 / MAIN_LOOP_DELAY) == 0) {
-			/* send parameters at 20 Hz (if queued for sending) */
+		if (fast_rate_limiter.check(t)) {
 			mavlink_pm_queued_send();
 			mavlink_waypoint_eventloop(hrt_absolute_time());
 
-			if (_baudrate > 57600) {
-				mavlink_pm_queued_send();
-			}
-
-			 /* send one string at 20 Hz */
 			if (!mavlink_logbuffer_is_empty(&lb)) {
 				struct mavlink_logmessage msg;
 				int lb_ret = mavlink_logbuffer_read(&lb, &msg);
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index e7f3486da..506b4317a 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -201,7 +201,7 @@ public:
 	 */
 	int		set_hil_enabled(bool hil_enabled);
 
-	MavlinkOrbSubscription *add_orb_subscription(const struct orb_metadata *topic, size_t size);
+	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, size_t size);
 
 	mavlink_channel_t get_channel();
 
@@ -231,10 +231,7 @@ private:
 	MAVLINK_MODE _mode;
 
 	uint8_t _mavlink_wpm_comp_id;
-	mavlink_channel_t _chan;
-
-//  XXX probably should be in a header... 
-// extern pthread_t receive_start(int uart);
+	mavlink_channel_t _channel;
 
 	struct mavlink_logbuffer lb;
 	unsigned int total_counter;
@@ -248,7 +245,7 @@ private:
 	bool _verbose;
 	int _uart;
 	int _baudrate;
-	bool gcs_link;
+
 	/**
 	 * If the queue index is not at 0, the queue sending
 	 * logic will send parameters from the current index
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 48443fbdc..967606841 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -5,6 +5,7 @@
  *      Author: ton
  */
 
+#include <stdio.h>
 #include <commander/px4_custom_mode.h>
 #include <lib/geo/geo.h>
 
@@ -19,6 +20,10 @@
 #include "mavlink_messages.h"
 
 
+static uint16_t cm_uint16_from_m_float(float m);
+static void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
+		uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
+
 uint16_t
 cm_uint16_from_m_float(float m)
 {
@@ -32,6 +37,83 @@ cm_uint16_from_m_float(float m)
 	return (uint16_t)(m * 100.0f);
 }
 
+void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet,
+		uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
+{
+	*mavlink_state = 0;
+	*mavlink_base_mode = 0;
+	*mavlink_custom_mode = 0;
+
+	/* HIL */
+	if (status->hil_state == HIL_STATE_ON) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+	}
+
+	/* arming state */
+	if (status->arming_state == ARMING_STATE_ARMED
+			|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+	}
+
+	/* main state */
+	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+
+	union px4_custom_mode custom_mode;
+	custom_mode.data = 0;
+	if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
+	/* use main state when navigator is not active */
+		if (status->main_state == MAIN_STATE_MANUAL) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+		} else if (status->main_state == MAIN_STATE_SEATBELT) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
+		} else if (status->main_state == MAIN_STATE_EASY) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
+		} else if (status->main_state == MAIN_STATE_AUTO) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+		}
+	} else {
+		/* use navigation state when navigator is active */
+		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		if (pos_sp_triplet->nav_state == NAV_STATE_READY) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+		} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+		} else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+		} else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+		} else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
+		}
+	}
+
+	*mavlink_custom_mode = custom_mode.data;
+
+	/* set system state */
+	if (status->arming_state == ARMING_STATE_INIT
+			|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE
+			|| status->arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
+		*mavlink_state = MAV_STATE_UNINIT;
+	} else if (status->arming_state == ARMING_STATE_ARMED) {
+		*mavlink_state = MAV_STATE_ACTIVE;
+	} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_state = MAV_STATE_CRITICAL;
+	} else if (status->arming_state == ARMING_STATE_STANDBY) {
+		*mavlink_state = MAV_STATE_STANDBY;
+	} else if (status->arming_state == ARMING_STATE_REBOOT) {
+		*mavlink_state = MAV_STATE_POWEROFF;
+	} else {
+		*mavlink_state = MAV_STATE_CRITICAL;
+	}
+}
+
+
 class MavlinkStreamHeartbeat : public MavlinkStream {
 public:
 	const char *get_name()
@@ -68,78 +150,13 @@ protected:
 		uint8_t mavlink_state = 0;
 		uint8_t mavlink_base_mode = 0;
 		uint32_t mavlink_custom_mode = 0;
-
-		/* HIL */
-		if (status->hil_state == HIL_STATE_ON) {
-			mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
-		}
-
-		/* arming state */
-		if (status->arming_state == ARMING_STATE_ARMED
-				|| status->arming_state == ARMING_STATE_ARMED_ERROR) {
-			mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-		}
-
-		/* main state */
-		mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
-
-		union px4_custom_mode custom_mode;
-		custom_mode.data = 0;
-		if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
-		/* use main state when navigator is not active */
-			if (status->main_state == MAIN_STATE_MANUAL) {
-				mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
-				custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
-			} else if (status->main_state == MAIN_STATE_SEATBELT) {
-				mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
-				custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
-			} else if (status->main_state == MAIN_STATE_EASY) {
-				mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-				custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
-			} else if (status->main_state == MAIN_STATE_AUTO) {
-				mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-				custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-				custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-			}
-		} else {
-			/* use navigation state when navigator is active */
-			mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-			if (pos_sp_triplet->nav_state == NAV_STATE_READY) {
-				custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-			} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
-				custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
-			} else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) {
-				custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
-			} else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) {
-				custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
-			} else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) {
-				custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
-			}
-		}
-
-		/* set system state */
-		if (status->arming_state == ARMING_STATE_INIT
-				|| status->arming_state == ARMING_STATE_IN_AIR_RESTORE
-				|| status->arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
-			mavlink_state = MAV_STATE_UNINIT;
-		} else if (status->arming_state == ARMING_STATE_ARMED) {
-			mavlink_state = MAV_STATE_ACTIVE;
-		} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
-			mavlink_state = MAV_STATE_CRITICAL;
-		} else if (status->arming_state == ARMING_STATE_STANDBY) {
-			mavlink_state = MAV_STATE_STANDBY;
-		} else if (status->arming_state == ARMING_STATE_REBOOT) {
-			mavlink_state = MAV_STATE_POWEROFF;
-		} else {
-			mavlink_state = MAV_STATE_CRITICAL;
-		}
+		get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 		mavlink_msg_heartbeat_send(_channel,
 					   mavlink_system.type,
 					   MAV_AUTOPILOT_PX4,
 					   mavlink_base_mode,
-					   custom_mode.data,
+					   mavlink_custom_mode,
 					   mavlink_state);
 
 	}
@@ -164,7 +181,6 @@ private:
 	struct vehicle_status_s *status;
 
 protected:
-
 	void subscribe(Mavlink *mavlink)
 	{
 		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s));
@@ -215,7 +231,6 @@ private:
 	uint32_t baro_counter = 0;
 
 protected:
-
 	void subscribe(Mavlink *mavlink)
 	{
 		sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined), sizeof(struct sensor_combined_s));
@@ -281,7 +296,6 @@ private:
 	struct vehicle_attitude_s *att;
 
 protected:
-
 	void subscribe(Mavlink *mavlink)
 	{
 		att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude), sizeof(struct vehicle_attitude_s));
@@ -313,11 +327,9 @@ public:
 
 private:
 	MavlinkOrbSubscription *gps_sub;
-
 	struct vehicle_gps_position_s *gps;
 
 protected:
-
 	void subscribe(Mavlink *mavlink)
 	{
 		gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position), sizeof(struct vehicle_gps_position_s));
@@ -356,13 +368,12 @@ public:
 
 private:
 	MavlinkOrbSubscription *pos_sub;
-	MavlinkOrbSubscription *home_sub;
-
 	struct vehicle_global_position_s *pos;
+
+	MavlinkOrbSubscription *home_sub;
 	struct home_position_s *home;
 
 protected:
-
 	void subscribe(Mavlink *mavlink)
 	{
 		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position), sizeof(struct vehicle_global_position_s));
@@ -404,11 +415,9 @@ public:
 
 private:
 	MavlinkOrbSubscription *pos_sub;
-
 	struct vehicle_local_position_s *pos;
 
 protected:
-
 	void subscribe(Mavlink *mavlink)
 	{
 		pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position), sizeof(struct vehicle_local_position_s));
@@ -444,11 +453,9 @@ public:
 
 private:
 	MavlinkOrbSubscription *home_sub;
-
 	struct home_position_s *home;
 
 protected:
-
 	void subscribe(Mavlink *mavlink)
 	{
 		home_sub = mavlink->add_orb_subscription(ORB_ID(home_position), sizeof(struct home_position_s));
@@ -466,6 +473,171 @@ protected:
 };
 
 
+class MavlinkStreamServoOutputRaw : public MavlinkStream {
+public:
+	MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n)
+	{
+		sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n);
+	}
+
+	const char *get_name()
+	{
+		return _name;
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamServoOutputRaw(_n);
+	}
+
+private:
+	MavlinkOrbSubscription *act_sub;
+	struct actuator_outputs_s *act;
+
+	char _name[20];
+	unsigned int _n;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		orb_id_t act_topics[] = {
+			ORB_ID(actuator_outputs_0),
+			ORB_ID(actuator_outputs_1),
+			ORB_ID(actuator_outputs_2),
+			ORB_ID(actuator_outputs_3)
+		};
+
+		act_sub = mavlink->add_orb_subscription(act_topics[_n], sizeof(struct actuator_outputs_s));
+		act = (struct actuator_outputs_s *)act_sub->get_data();
+	}
+
+	void send(const hrt_abstime t) {
+		act_sub->update(t);
+
+		mavlink_msg_servo_output_raw_send(_channel,
+				act->timestamp / 1000,
+				_n,
+				act->output[0],
+				act->output[1],
+				act->output[2],
+				act->output[3],
+				act->output[4],
+				act->output[5],
+				act->output[6],
+				act->output[7]);
+	}
+};
+
+
+class MavlinkStreamHILControls : public MavlinkStream {
+public:
+	const char *get_name()
+	{
+		return "HIL_CONTROLS";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamHILControls();
+	}
+
+private:
+	MavlinkOrbSubscription *status_sub;
+	struct vehicle_status_s *status;
+
+	MavlinkOrbSubscription *pos_sp_triplet_sub;
+	struct position_setpoint_triplet_s *pos_sp_triplet;
+
+	MavlinkOrbSubscription *act_sub;
+	struct actuator_outputs_s *act;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status), sizeof(struct vehicle_status_s));
+		status = (struct vehicle_status_s *)status_sub->get_data();
+
+		pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s));
+		pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
+
+		act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0), sizeof(struct actuator_outputs_s));
+		act = (struct actuator_outputs_s *)act_sub->get_data();
+	}
+
+	void send(const hrt_abstime t) {
+		status_sub->update(t);
+		pos_sp_triplet_sub->update(t);
+		act_sub->update(t);
+
+		/* translate the current syste state to mavlink state and mode */
+		uint8_t mavlink_state;
+		uint8_t mavlink_base_mode;
+		uint32_t mavlink_custom_mode;
+		get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+
+		/* HIL message as per MAVLink spec */
+
+		/* scale / assign outputs depending on system type */
+		if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
+			mavlink_msg_hil_controls_send(_channel,
+							  hrt_absolute_time(),
+							  ((act->output[0] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[1] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[2] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[3] - 900.0f) / 600.0f) / 2.0f,
+							  -1,
+							  -1,
+							  -1,
+							  -1,
+							  mavlink_base_mode,
+							  0);
+
+		} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
+			mavlink_msg_hil_controls_send(_channel,
+							  hrt_absolute_time(),
+							  ((act->output[0] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[1] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[2] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[3] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[4] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[5] - 900.0f) / 600.0f) / 2.0f,
+							  -1,
+							  -1,
+							  mavlink_base_mode,
+							  0);
+
+		} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
+			mavlink_msg_hil_controls_send(_channel,
+							  hrt_absolute_time(),
+							  ((act->output[0] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[1] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[2] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[3] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[4] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[5] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[6] - 900.0f) / 600.0f) / 2.0f,
+							  ((act->output[7] - 900.0f) / 600.0f) / 2.0f,
+							  mavlink_base_mode,
+							  0);
+
+		} else {
+			mavlink_msg_hil_controls_send(_channel,
+							  hrt_absolute_time(),
+							  (act->output[0] - 1500.0f) / 500.0f,
+							  (act->output[1] - 1500.0f) / 500.0f,
+							  (act->output[2] - 1500.0f) / 500.0f,
+							  (act->output[3] - 1000.0f) / 1000.0f,
+							  (act->output[4] - 1500.0f) / 500.0f,
+							  (act->output[5] - 1500.0f) / 500.0f,
+							  (act->output[6] - 1500.0f) / 500.0f,
+							  (act->output[7] - 1500.0f) / 500.0f,
+							  mavlink_base_mode,
+							  0);
+		}
+	}
+};
+
+
 MavlinkStream *streams_list[] = {
 		new MavlinkStreamHeartbeat(),
 		new MavlinkStreamSysStatus(),
@@ -475,6 +647,11 @@ MavlinkStream *streams_list[] = {
 		new MavlinkStreamGlobalPositionInt(),
 		new MavlinkStreamLocalPositionNED(),
 		new MavlinkStreamGPSGlobalOrigin(),
+		new MavlinkStreamServoOutputRaw(0),
+		new MavlinkStreamServoOutputRaw(1),
+		new MavlinkStreamServoOutputRaw(2),
+		new MavlinkStreamServoOutputRaw(3),
+		new MavlinkStreamHILControls(),
 		nullptr
 };
 
@@ -484,8 +661,6 @@ MavlinkStream *streams_list[] = {
 
 
 
-
-
 //void
 //MavlinkOrbListener::l_vehicle_attitude(const struct listener *l)
 //{
@@ -652,107 +827,6 @@ MavlinkStream *streams_list[] = {
 //}
 //
 //void
-//MavlinkOrbListener::l_actuator_outputs(const struct listener *l)
-//{
-//	struct actuator_outputs_s act_outputs;
-//
-//	orb_id_t ids[] = {
-//		ORB_ID(actuator_outputs_0),
-//		ORB_ID(actuator_outputs_1),
-//		ORB_ID(actuator_outputs_2),
-//		ORB_ID(actuator_outputs_3)
-//	};
-//
-//	/* copy actuator data into local buffer */
-//	orb_copy(ids[l->arg], *l->subp, &act_outputs);
-//
-//	if (l->mavlink->get_mode() == Mavlink::MODE_OFFBOARD) {
-//		mavlink_msg_servo_output_raw_send(l->mavlink->get_chan(), l->listener->last_sensor_timestamp / 1000,
-//						  l->arg /* port number - needs GCS support */,
-//							 /* QGC has port number support already */
-//						  act_outputs.output[0],
-//						  act_outputs.output[1],
-//						  act_outputs.output[2],
-//						  act_outputs.output[3],
-//						  act_outputs.output[4],
-//						  act_outputs.output[5],
-//						  act_outputs.output[6],
-//						  act_outputs.output[7]);
-//
-//		/* only send in HIL mode and only send first group for HIL */
-//		if (l->mavlink->get_hil_enabled() && l->listener->armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
-//
-//			/* translate the current syste state to mavlink state and mode */
-//			uint8_t mavlink_state = 0;
-//			uint8_t mavlink_base_mode = 0;
-//			uint32_t mavlink_custom_mode = 0;
-//			l->mavlink->get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-//
-//			/* HIL message as per MAVLink spec */
-//
-//			/* scale / assign outputs depending on system type */
-//
-//			if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
-//				mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
-//							      hrt_absolute_time(),
-//							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-//							      -1,
-//							      -1,
-//							      -1,
-//							      -1,
-//							      mavlink_base_mode,
-//							      0);
-//
-//			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
-//				mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
-//							      hrt_absolute_time(),
-//							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-//							      -1,
-//							      -1,
-//							      mavlink_base_mode,
-//							      0);
-//
-//			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
-//				mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
-//							      hrt_absolute_time(),
-//							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
-//							      ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
-//							      mavlink_base_mode,
-//							      0);
-//
-//			} else {
-//				mavlink_msg_hil_controls_send(l->mavlink->get_chan(),
-//							      hrt_absolute_time(),
-//							      (act_outputs.output[0] - 1500.0f) / 500.0f,
-//							      (act_outputs.output[1] - 1500.0f) / 500.0f,
-//							      (act_outputs.output[2] - 1500.0f) / 500.0f,
-//							      (act_outputs.output[3] - 1000.0f) / 1000.0f,
-//							      (act_outputs.output[4] - 1500.0f) / 500.0f,
-//							      (act_outputs.output[5] - 1500.0f) / 500.0f,
-//							      (act_outputs.output[6] - 1500.0f) / 500.0f,
-//							      (act_outputs.output[7] - 1500.0f) / 500.0f,
-//							      mavlink_base_mode,
-//							      0);
-//			}
-//		}
-//	}
-//}
-//
-//void
 //MavlinkOrbListener::l_actuator_armed(const struct listener *l)
 //{
 //	orb_copy(ORB_ID(actuator_armed), l->mavlink->get_subs()->armed_sub, &l->listener->armed);
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index b504b6955..35470a19a 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -13,7 +13,7 @@
 
 #include "mavlink_orb_subscription.h"
 
-MavlinkOrbSubscription::MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size) : _topic(topic), _last_check(0), next(nullptr)
+MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, size_t size) : _topic(topic), _last_check(0), next(nullptr)
 {
 	_data = malloc(size);
 	memset(_data, 0, size);
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index c38a9cc43..79ff3abdb 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -16,7 +16,7 @@ class MavlinkOrbSubscription {
 public:
 	MavlinkOrbSubscription *next;
 
-	MavlinkOrbSubscription(const struct orb_metadata *topic, size_t size);
+	MavlinkOrbSubscription(const orb_id_t topic, size_t size);
 	~MavlinkOrbSubscription();
 
 	bool update(const hrt_abstime t);
@@ -24,7 +24,7 @@ public:
 	const struct orb_metadata *get_topic();
 
 private:
-	const struct orb_metadata *_topic;
+	const orb_id_t _topic;
 	int _fd;
 	void *_data;
 	hrt_abstime _last_check;
diff --git a/src/modules/mavlink/mavlink_rate_limiter.cpp b/src/modules/mavlink/mavlink_rate_limiter.cpp
new file mode 100644
index 000000000..f5bb06ccd
--- /dev/null
+++ b/src/modules/mavlink/mavlink_rate_limiter.cpp
@@ -0,0 +1,38 @@
+/*
+ * mavlink_rate_limiter.cpp
+ *
+ *  Created on: 26.02.2014
+ *      Author: ton
+ */
+
+
+#include "mavlink_rate_limiter.h"
+
+MavlinkRateLimiter::MavlinkRateLimiter() : _last_sent(0), _interval(1000000)
+{
+}
+
+MavlinkRateLimiter::MavlinkRateLimiter(unsigned int interval) : _last_sent(0), _interval(interval * 1000)
+{
+}
+
+MavlinkRateLimiter::~MavlinkRateLimiter()
+{
+}
+
+void
+MavlinkRateLimiter::set_interval(unsigned int interval)
+{
+	_interval = interval * 1000;
+}
+
+bool
+MavlinkRateLimiter::check(hrt_abstime t)
+{
+	uint64_t dt = t - _last_sent;
+	if (dt > 0 && dt >= _interval) {
+		_last_sent = (t / _interval) * _interval;
+		return true;
+	}
+	return false;
+}
diff --git a/src/modules/mavlink/mavlink_rate_limiter.h b/src/modules/mavlink/mavlink_rate_limiter.h
new file mode 100644
index 000000000..6db65f638
--- /dev/null
+++ b/src/modules/mavlink/mavlink_rate_limiter.h
@@ -0,0 +1,28 @@
+/*
+ * mavlink_rate_limiter.h
+ *
+ *  Created on: 26.02.2014
+ *      Author: ton
+ */
+
+#ifndef MAVLINK_RATE_LIMITER_H_
+#define MAVLINK_RATE_LIMITER_H_
+
+#include <drivers/drv_hrt.h>
+
+
+class MavlinkRateLimiter {
+private:
+	hrt_abstime _last_sent;
+	hrt_abstime _interval;
+
+public:
+	MavlinkRateLimiter();
+	MavlinkRateLimiter(unsigned int interval);
+	~MavlinkRateLimiter();
+	void set_interval(unsigned int interval);
+	bool check(hrt_abstime t);
+};
+
+
+#endif /* MAVLINK_RATE_LIMITER_H_ */
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index 16407366e..703f74b4c 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -10,7 +10,7 @@
 #include "mavlink_stream.h"
 #include "mavlink_main.h"
 
-MavlinkStream::MavlinkStream() : _interval(1000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr)
+MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr)
 {
 }
 
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 5ea6c0f46..f09efa2e6 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -41,6 +41,7 @@ SRCS		 += mavlink_main.cpp \
 			mavlink_receiver.cpp \
 			mavlink_orb_subscription.cpp \
 			mavlink_messages.cpp \
-			mavlink_stream.cpp
+			mavlink_stream.cpp \
+			mavlink_rate_limiter.cpp
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink