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

1 files changed, 256 insertions, 182 deletions

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);