Merged origin/master into batt_fixes

3 files changed, 130 insertions, 113 deletions

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c51a6de08..7b6fad658 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -72,6 +72,7 @@
 #include <systemlib/airspeed.h>
 #include <mavlink/mavlink_log.h>
 #include <commander/px4_custom_mode.h>
+#include <geo/geo.h>
 
 __BEGIN_DECLS
 
@@ -99,11 +100,13 @@ static struct vehicle_command_s vcmd;
 static struct offboard_control_setpoint_s offboard_control_sp;
 
 struct vehicle_global_position_s hil_global_pos;
+struct vehicle_local_position_s hil_local_pos;
 struct vehicle_attitude_s hil_attitude;
 struct vehicle_gps_position_s hil_gps;
 struct sensor_combined_s hil_sensors;
 struct battery_status_s	hil_battery_status;
 static orb_advert_t pub_hil_global_pos = -1;
+static orb_advert_t pub_hil_local_pos = -1;
 static orb_advert_t pub_hil_attitude = -1;
 static orb_advert_t pub_hil_gps = -1;
 static orb_advert_t pub_hil_sensors = -1;
@@ -126,6 +129,11 @@ static orb_advert_t offboard_control_sp_pub = -1;
 static orb_advert_t vicon_position_pub = -1;
 static orb_advert_t telemetry_status_pub = -1;
 
+// variables for HIL reference position
+static int32_t lat0 = 0;
+static int32_t lon0 = 0;
+static double alt0 = 0;
+
 static void
 handle_message(mavlink_message_t *msg)
 {
@@ -374,16 +382,15 @@ handle_message(mavlink_message_t *msg)
 
 			/* hil gyro */
 			static const float mrad2rad = 1.0e-3f;
-			hil_sensors.gyro_counter = hil_counter;
 			hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad;
 			hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad;
 			hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad;
 			hil_sensors.gyro_rad_s[0] = imu.xgyro;
 			hil_sensors.gyro_rad_s[1] = imu.ygyro;
 			hil_sensors.gyro_rad_s[2] = imu.zgyro;
+			hil_sensors.gyro_counter = hil_counter;
 
 			/* accelerometer */
-			hil_sensors.accelerometer_counter = hil_counter;
 			static const float mg2ms2 = 9.8f / 1000.0f;
 			hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
 			hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
@@ -393,6 +400,7 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.accelerometer_m_s2[2] = imu.zacc;
 			hil_sensors.accelerometer_mode = 0; // TODO what is this?
 			hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
+			hil_sensors.accelerometer_counter = hil_counter;
 
 			/* adc */
 			hil_sensors.adc_voltage_v[0] = 0.0f;
@@ -401,7 +409,6 @@ handle_message(mavlink_message_t *msg)
 
 			/* magnetometer */
 			float mga2ga = 1.0e-3f;
-			hil_sensors.magnetometer_counter = hil_counter;
 			hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga;
 			hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
 			hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga;
@@ -411,15 +418,13 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.magnetometer_range_ga = 32.7f; // int16
 			hil_sensors.magnetometer_mode = 0; // TODO what is this
 			hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
+			hil_sensors.magnetometer_counter = hil_counter;
 
 			/* baro */
 			hil_sensors.baro_pres_mbar = imu.abs_pressure;
 			hil_sensors.baro_alt_meter = imu.pressure_alt;
 			hil_sensors.baro_temp_celcius = imu.temperature;
-
-			hil_sensors.gyro_counter = hil_counter;
-			hil_sensors.magnetometer_counter = hil_counter;
-			hil_sensors.accelerometer_counter = hil_counter;
+			hil_sensors.baro_counter = hil_counter;
 
 			/* differential pressure */
 			hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
@@ -621,24 +626,61 @@ handle_message(mavlink_message_t *msg)
 				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
 
-			hil_global_pos.valid = true;
-			hil_global_pos.lat = hil_state.lat;
-			hil_global_pos.lon = hil_state.lon;
-			hil_global_pos.alt = hil_state.alt / 1000.0f;
-			hil_global_pos.vx = hil_state.vx / 100.0f;
-			hil_global_pos.vy = hil_state.vy / 100.0f;
-			hil_global_pos.vz = hil_state.vz / 100.0f;
-
-			/* set timestamp and notify processes (broadcast) */
-			hil_global_pos.timestamp = hrt_absolute_time();
+			uint64_t timestamp = hrt_absolute_time();
 
+			// publish global position	
 			if (pub_hil_global_pos > 0) {
 				orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
+				// global position packet
+				hil_global_pos.timestamp = timestamp;
+				hil_global_pos.valid = true;
+				hil_global_pos.lat = hil_state.lat;
+				hil_global_pos.lon = hil_state.lon;
+				hil_global_pos.alt = hil_state.alt / 1000.0f;
+				hil_global_pos.vx = hil_state.vx / 100.0f;
+				hil_global_pos.vy = hil_state.vy / 100.0f;
+				hil_global_pos.vz = hil_state.vz / 100.0f;
 
 			} else {
 				pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
 			}
 
+			// publish local position
+			if (pub_hil_local_pos > 0) {
+				float x;
+				float y;
+				bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve?
+				double lat = hil_state.lat*1e-7;
+				double lon = hil_state.lon*1e-7;
+				map_projection_project(lat, lon, &x, &y); 
+				hil_local_pos.timestamp = timestamp;
+				hil_local_pos.xy_valid = true;
+				hil_local_pos.z_valid = true;
+				hil_local_pos.v_xy_valid = true;
+				hil_local_pos.v_z_valid = true;
+				hil_local_pos.x = x;
+				hil_local_pos.y = y;
+				hil_local_pos.z = alt0 - hil_state.alt/1000.0f;
+				hil_local_pos.vx = hil_state.vx/100.0f;
+				hil_local_pos.vy = hil_state.vy/100.0f;
+				hil_local_pos.vz = hil_state.vz/100.0f;
+				hil_local_pos.yaw = hil_attitude.yaw;
+				hil_local_pos.xy_global = true;
+				hil_local_pos.z_global = true;
+				hil_local_pos.ref_timestamp = timestamp;
+				hil_local_pos.ref_lat = hil_state.lat;
+				hil_local_pos.ref_lon = hil_state.lon;
+				hil_local_pos.ref_alt = alt0;
+				hil_local_pos.landed = landed;
+				orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos);
+			} else {
+				pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
+				lat0 = hil_state.lat;
+				lon0 = hil_state.lon;
+				alt0 = hil_state.alt / 1000.0f;
+				map_projection_init(hil_state.lat, hil_state.lon);
+			}
+
 			/* Calculate Rotation Matrix */
 			math::Quaternion q(hil_state.attitude_quaternion);
 			math::Dcm C_nb(q);
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index fa23f996f..124b3b2ae 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -276,7 +276,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 			next_setpoint_index = index + 1;
 		}
 
-		while (next_setpoint_index < wpm->size - 1) {
+		while (next_setpoint_index < wpm->size) {
 
 			if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
 					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
@@ -301,7 +301,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 			sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
 			sp.altitude = wpm->waypoints[last_setpoint_index].z;
 			sp.altitude_is_relative = false;
-			sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
+			sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
 			set_special_fields(wpm->waypoints[last_setpoint_index].param1,
 				wpm->waypoints[last_setpoint_index].param2,
 				wpm->waypoints[last_setpoint_index].param3,
@@ -317,7 +317,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 			sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
 			sp.altitude = wpm->waypoints[next_setpoint_index].z;
 			sp.altitude_is_relative = false;
-			sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
+			sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
 			set_special_fields(wpm->waypoints[next_setpoint_index].param1,
 				wpm->waypoints[next_setpoint_index].param2,
 				wpm->waypoints[next_setpoint_index].param3,
@@ -343,7 +343,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		sp.lon = param6_lon_y * 1e7f;
 		sp.altitude = param7_alt_z;
 		sp.altitude_is_relative = true;
-		sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
 		set_special_fields(param1, param2, param3, param4, command, &sp);
 
 		/* Initialize publication if necessary */
@@ -364,7 +364,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		sp.x = param5_lat_x;
 		sp.y = param6_lon_y;
 		sp.z = param7_alt_z;
-		sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+		sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
 
 		/* Initialize publication if necessary */
 		if (local_position_setpoint_pub < 0) {
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index f6860930c..92b1b45be 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -54,6 +54,7 @@
 #include <sys/prctl.h>
 #include <stdlib.h>
 #include <poll.h>
+#include <lib/geo/geo.h>
 
 #include <mavlink/mavlink_log.h>
 
@@ -72,9 +73,9 @@ struct vehicle_status_s v_status;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
-struct actuator_controls_effective_s actuators_effective_0;
 struct actuator_controls_s actuators_0;
 struct vehicle_attitude_s att;
+struct airspeed_s airspeed;
 
 struct mavlink_subscriptions mavlink_subs;
 
@@ -92,6 +93,8 @@ static unsigned int gps_counter;
  */
 static uint64_t last_sensor_timestamp;
 
+static hrt_abstime last_sent_vfr = 0;
+
 static void	*uorb_receive_thread(void *arg);
 
 struct listener {
@@ -116,7 +119,6 @@ static void	l_attitude_setpoint(const struct listener *l);
 static void	l_actuator_outputs(const struct listener *l);
 static void	l_actuator_armed(const struct listener *l);
 static void	l_manual_control_setpoint(const struct listener *l);
-static void	l_vehicle_attitude_controls_effective(const struct listener *l);
 static void	l_vehicle_attitude_controls(const struct listener *l);
 static void	l_debug_key_value(const struct listener *l);
 static void	l_optical_flow(const struct listener *l);
@@ -144,7 +146,6 @@ static const struct listener listeners[] = {
 	{l_actuator_armed,		&mavlink_subs.armed_sub,	0},
 	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
 	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
-	{l_vehicle_attitude_controls_effective,	&mavlink_subs.actuators_effective_sub,	0},
 	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
 	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
 	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
@@ -229,7 +230,7 @@ l_vehicle_attitude(const struct listener *l)
 	/* copy attitude data into local buffer */
 	orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
 
-	if (gcs_link)
+	if (gcs_link) {
 		/* send sensor values */
 		mavlink_msg_attitude_send(MAVLINK_COMM_0,
 					  last_sensor_timestamp / 1000,
@@ -239,6 +240,30 @@ l_vehicle_attitude(const struct listener *l)
 					  att.rollspeed,
 					  att.pitchspeed,
 					  att.yawspeed);
+					  	
+		/* limit VFR message rate to 10Hz */
+		hrt_abstime t = hrt_absolute_time();
+		if (t >= last_sent_vfr + 100000) {
+			last_sent_vfr = t;
+			float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
+			uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
+			float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
+			mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
+		}
+		
+		/* send quaternion values if it exists */
+		if(att.q_valid) {
+			mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
+												last_sensor_timestamp / 1000,
+												att.q[0],
+												att.q[1],
+												att.q[2],
+												att.q[3],
+												att.rollspeed,
+												att.pitchspeed,
+												att.yawspeed);
+		}
+	}
 
 	attitude_counter++;
 }
@@ -252,13 +277,7 @@ l_vehicle_gps_position(const struct listener *l)
 	orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
 
 	/* GPS COG is 0..2PI in degrees * 1e2 */
-	float cog_deg = gps.cog_rad;
-
-	if (cog_deg > M_PI_F)
-		cog_deg -= 2.0f * M_PI_F;
-
-	cog_deg *= M_RAD_TO_DEG_F;
-
+	float cog_deg = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F;
 
 	/* GPS position */
 	mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
@@ -329,20 +348,26 @@ l_input_rc(const struct listener *l)
 	/* copy rc channels into local buffer */
 	orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
 
-	if (gcs_link)
-		/* Channels are sent in MAVLink main loop at a fixed interval */
-		mavlink_msg_rc_channels_raw_send(chan,
-						 rc_raw.timestamp / 1000,
-						 0,
-						 (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX,
-						 (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX,
-						 (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX,
-						 (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX,
-						 (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX,
-						 (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX,
-						 (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX,
-						 (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX,
-						 255);
+	if (gcs_link) {
+
+		const unsigned port_width = 8;
+
+		for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) {
+			/* Channels are sent in MAVLink main loop at a fixed interval */
+			mavlink_msg_rc_channels_raw_send(chan,
+							 rc_raw.timestamp / 1000,
+							 i,
+							 (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX,
+							 (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX,
+							 (rc_raw.channel_count > (i * port_width) + 2) ? rc_raw.values[(i * port_width) + 2] : UINT16_MAX,
+							 (rc_raw.channel_count > (i * port_width) + 3) ? rc_raw.values[(i * port_width) + 3] : UINT16_MAX,
+							 (rc_raw.channel_count > (i * port_width) + 4) ? rc_raw.values[(i * port_width) + 4] : UINT16_MAX,
+							 (rc_raw.channel_count > (i * port_width) + 5) ? rc_raw.values[(i * port_width) + 5] : UINT16_MAX,
+							 (rc_raw.channel_count > (i * port_width) + 6) ? rc_raw.values[(i * port_width) + 6] : UINT16_MAX,
+							 (rc_raw.channel_count > (i * port_width) + 7) ? rc_raw.values[(i * port_width) + 7] : UINT16_MAX,
+							 rc_raw.rssi);
+		}
+	}
 }
 
 void
@@ -351,28 +376,16 @@ l_global_position(const struct listener *l)
 	/* copy global position data into local buffer */
 	orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
 
-	uint64_t timestamp = global_pos.timestamp;
-	int32_t lat = global_pos.lat;
-	int32_t lon = global_pos.lon;
-	int32_t alt = (int32_t)(global_pos.alt * 1000);
-	int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f);
-	int16_t vx = (int16_t)(global_pos.vx * 100.0f);
-	int16_t vy = (int16_t)(global_pos.vy * 100.0f);
-	int16_t vz = (int16_t)(global_pos.vz * 100.0f);
-
-	/* heading in degrees * 10, from 0 to 36.000) */
-	uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
-
 	mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
-					     timestamp / 1000,
-					     lat,
-					     lon,
-					     alt,
-					     relative_alt,
-					     vx,
-					     vy,
-					     vz,
-					     hdg);
+						 global_pos.timestamp / 1000,
+					     global_pos.lat,
+					     global_pos.lon,
+					     global_pos.alt * 1000.0f,
+					     global_pos.relative_alt * 1000.0f,
+					     global_pos.vx * 100.0f,
+					     global_pos.vy * 100.0f,
+					     global_pos.vz * 100.0f,
+					     _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
 }
 
 void
@@ -410,8 +423,8 @@ l_global_position_setpoint(const struct listener *l)
 				coordinate_frame,
 				global_sp.lat,
 				global_sp.lon,
-				global_sp.altitude,
-				global_sp.yaw);
+				global_sp.altitude * 1000.0f,
+				global_sp.yaw * M_RAD_TO_DEG_F * 100.0f);
 }
 
 void
@@ -482,7 +495,8 @@ l_actuator_outputs(const struct listener *l)
 
 	if (gcs_link) {
 		mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
-						  l->arg /* port number */,
+						  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],
@@ -492,8 +506,8 @@ l_actuator_outputs(const struct listener *l)
 						  act_outputs.output[6],
 						  act_outputs.output[7]);
 
-		/* only send in HIL mode */
-		if (mavlink_hil_enabled && armed.armed) {
+		/* only send in HIL mode and only send first group for HIL */
+		if (mavlink_hil_enabled && armed.armed && ids[l->arg] == ORB_ID(actuator_outputs_0)) {
 
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
@@ -590,32 +604,6 @@ l_manual_control_setpoint(const struct listener *l)
 }
 
 void
-l_vehicle_attitude_controls_effective(const struct listener *l)
-{
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
-
-	if (gcs_link) {
-		/* send, add spaces so that string buffer is at least 10 chars long */
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl0    ",
-						   actuators_effective_0.control_effective[0]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl1    ",
-						   actuators_effective_0.control_effective[1]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl2     ",
-						   actuators_effective_0.control_effective[2]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl3     ",
-						   actuators_effective_0.control_effective[3]);
-	}
-}
-
-void
 l_vehicle_attitude_controls(const struct listener *l)
 {
 	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
@@ -681,17 +669,7 @@ l_home(const struct listener *l)
 void
 l_airspeed(const struct listener *l)
 {
-	struct airspeed_s airspeed;
-
 	orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
-
-	float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
-	uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
-	float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
-	float alt = global_pos.relative_alt;
-	float climb = -global_pos.vz;
-
-	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
 }
 
 void
@@ -835,9 +813,6 @@ uorb_receive_start(void)
 	orb_set_interval(mavlink_subs.man_control_sp_sub, 100);	/* 10Hz updates */
 
 	/* --- ACTUATOR CONTROL VALUE --- */
-	mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
-	orb_set_interval(mavlink_subs.actuators_effective_sub, 100);	/* 10Hz updates */
-
 	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */
 
@@ -849,7 +824,7 @@ uorb_receive_start(void)
 	mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
 	orb_set_interval(mavlink_subs.optical_flow, 200); 	/* 5Hz updates */
 
-	/* --- AIRSPEED / VFR / HUD --- */
+	/* --- AIRSPEED --- */
 	mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	orb_set_interval(mavlink_subs.airspeed_sub, 200); 	/* 5Hz updates */