Fixed a bunch of issues in the arming state machine for multirotors, arming / disarming works fine now. Porting of various processes needed

apps/mavlink/mavlink.c

@@ -73,6 +73,8 @@
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/optical_flow.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/manual_control_setpoint.h>
 #include "waypoints.h"
 #include "mavlink_log.h"
 
@@ -129,9 +131,6 @@ static struct vehicle_command_s vcmd;
 static orb_advert_t pub_hil_global_pos = -1;
 static orb_advert_t ardrone_motors_pub = -1;
 static orb_advert_t cmd_pub = -1;
-static int sensor_sub = -1;
-static int att_sub = -1;
-static int global_pos_sub = -1;
 static int local_pos_sub = -1;
 static orb_advert_t flow_pub = -1;
 static orb_advert_t global_position_setpoint_pub = -1;
@@ -146,6 +145,30 @@ static enum {
 	MAVLINK_INTERFACE_MODE_ONBOARD
 } mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD;
 
+static struct mavlink_subscriptions {
+	int sensor_sub;
+	int att_sub;
+	int global_pos_sub;
+	int act_0_sub;
+	int act_1_sub;
+	int act_2_sub;
+	int act_3_sub;
+	int gps_sub;
+	int man_control_sp_sub;
+	bool initialized;
+} mavlink_subs = {
+	.sensor_sub = 0,
+	.att_sub = 0,
+	.global_pos_sub = 0,
+	.act_0_sub = 0,
+	.act_1_sub = 0,
+	.act_2_sub = 0,
+	.act_3_sub = 0,
+	.gps_sub = 0,
+	.man_control_sp_sub = 0,
+	.initialized = false
+};
+
 
 /* 3: Define waypoint helper functions */
 void mavlink_wpm_send_message(mavlink_message_t *msg);
@@ -459,23 +482,33 @@ static void *receiveloop(void *arg)
 	return NULL;
 }
 
-static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval)
+static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
 {
 	int ret = OK;
 
 	switch (mavlink_msg_id) {
 		case MAVLINK_MSG_ID_SCALED_IMU:
 			/* senser sub triggers scaled IMU */
-			orb_set_interval(sensor_sub, min_interval);
+			if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval);
 			break;
 		case MAVLINK_MSG_ID_RAW_IMU:
 			/* senser sub triggers RAW IMU */
-			orb_set_interval(sensor_sub, min_interval);
+			if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval);
 			break;
 		case MAVLINK_MSG_ID_ATTITUDE:
 			/* attitude sub triggers attitude */
-			orb_set_interval(att_sub, min_interval);
+			if (subs->att_sub) orb_set_interval(subs->att_sub, min_interval);
 			break;
+		case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
+			/* actuator_outputs triggers this message */
+			if (subs->act_0_sub) orb_set_interval(subs->act_0_sub, min_interval);
+			if (subs->act_1_sub) orb_set_interval(subs->act_1_sub, min_interval);
+			if (subs->act_2_sub) orb_set_interval(subs->act_2_sub, min_interval);
+			if (subs->act_3_sub) orb_set_interval(subs->act_3_sub, min_interval);
+			break;
+		case MAVLINK_MSG_ID_MANUAL_CONTROL:
+			/* manual_control_setpoint triggers this message */
+			if (subs->man_control_sp_sub) orb_set_interval(subs->man_control_sp_sub, min_interval);
 		default:
 			/* not found */
 			ret = ERROR;
@@ -494,13 +527,16 @@ static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval)
  */
 static void *uorb_receiveloop(void *arg)
 {
+	/* obtain reference to task's subscriptions */
+	struct mavlink_subscriptions *subs = (struct mavlink_subscriptions *)arg;
+
 	/* Set thread name */
 	prctl(PR_SET_NAME, "mavlink uORB", getpid());
 
 
 	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
 	/* number of messages */
-	const ssize_t fdsc = 15;
+	const ssize_t fdsc = 25;
 	/* Sanity check variable and index */
 	ssize_t fdsc_count = 0;
 	/* file descriptors to wait for */
@@ -515,28 +551,30 @@ static void *uorb_receiveloop(void *arg)
 		struct vehicle_local_position_setpoint_s local_sp;
 		struct vehicle_global_position_setpoint_s global_sp;
 		struct vehicle_attitude_setpoint_s att_sp;
+		struct actuator_outputs_s act_outputs;
+		struct manual_control_setpoint_s man_control;
 	} buf;
 
 	/* --- SENSORS RAW VALUE --- */
 	/* subscribe to ORB for sensors raw */
-	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	subs->sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	fds[fdsc_count].fd = sensor_sub;
+	fds[fdsc_count].fd = subs->sensor_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
 	/* --- ATTITUDE VALUE --- */
 	/* subscribe to ORB for attitude */
-	att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	subs->att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	orb_set_interval(att_sub, 100);
+	orb_set_interval(subs->att_sub, 100);
-	fds[fdsc_count].fd = att_sub;
+	fds[fdsc_count].fd = subs->att_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
 	/* --- GPS VALUE --- */
 	/* subscribe to ORB for attitude */
-	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	subs->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	orb_set_interval(gps_sub, 1000);	/* 1Hz updates */
+	orb_set_interval(subs->gps_sub, 1000);	/* 1Hz updates */
-	fds[fdsc_count].fd = gps_sub;
+	fds[fdsc_count].fd = subs->gps_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
@@ -577,7 +615,7 @@ static void *uorb_receiveloop(void *arg)
 
 	/* --- GLOBAL POS VALUE --- */
 	/* struct already globally allocated and topic already subscribed */
-	fds[fdsc_count].fd = global_pos_sub;
+	fds[fdsc_count].fd = subs->global_pos_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
@@ -608,12 +646,39 @@ static void *uorb_receiveloop(void *arg)
 	/* --- ATTITUDE SETPOINT VALUE --- */
 	/* subscribe to ORB for attitude setpoint */
 	/* struct already allocated */
-	int spa_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
+	int spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	orb_set_interval(spa_sub, 2000);	/* 0.5 Hz updates */
 	fds[fdsc_count].fd = spa_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/** --- ACTUATOR OUTPUTS --- */
+	subs->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
+	fds[fdsc_count].fd = subs->act_0_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+	subs->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1));
+	fds[fdsc_count].fd = subs->act_1_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+	subs->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2));
+	fds[fdsc_count].fd = subs->act_2_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+	subs->act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3));
+	fds[fdsc_count].fd = subs->act_3_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
+	/** --- MAPPED MANUAL CONTROL INPUTS --- */
+	subs->man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	fds[fdsc_count].fd = subs->man_control_sp_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
+	/* all subscriptions initialized, return success */
+	subs->initialized = true;
+
 	unsigned int sensors_raw_counter = 0;
 	unsigned int attitude_counter = 0;
 	unsigned int gps_counter = 0;
@@ -651,7 +716,7 @@ static void *uorb_receiveloop(void *arg)
 			if (fds[ifds++].revents & POLLIN) {
 
 				/* copy sensors raw data into local buffer */
-				orb_copy(ORB_ID(sensor_combined), sensor_sub, &buf.raw);
+				orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &buf.raw);
 
 				/* send raw imu data */
 				mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0], buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]);
@@ -667,7 +732,7 @@ static void *uorb_receiveloop(void *arg)
 			if (fds[ifds++].revents & POLLIN) {
 
 				/* copy attitude data into local buffer */
-				orb_copy(ORB_ID(vehicle_attitude), att_sub, &buf.att);
+				orb_copy(ORB_ID(vehicle_attitude), subs->att_sub, &buf.att);
 
 				/* send sensor values */
 				mavlink_msg_attitude_send(MAVLINK_COMM_0, buf.att.timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed);
@@ -678,7 +743,7 @@ static void *uorb_receiveloop(void *arg)
 			/* --- GPS VALUE --- */
 			if (fds[ifds++].revents & POLLIN) {
 				/* copy gps data into local buffer */
-				orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &buf.gps);
+				orb_copy(ORB_ID(vehicle_gps_position), subs->gps_sub, &buf.gps);
 				/* GPS position */
 				mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, buf.gps.timestamp, buf.gps.fix_type, buf.gps.lat, buf.gps.lon, buf.gps.alt, buf.gps.eph, buf.gps.epv, buf.gps.vel, buf.gps.cog, buf.gps.satellites_visible);
 
@@ -801,7 +866,7 @@ static void *uorb_receiveloop(void *arg)
 			/* --- VEHICLE GLOBAL POSITION --- */
 			if (fds[ifds++].revents & POLLIN) {
 				/* copy global position data into local buffer */
-				orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
+				orb_copy(ORB_ID(vehicle_global_position), subs->global_pos_sub, &global_pos);
 				uint64_t timestamp = global_pos.timestamp;
 				int32_t lat = global_pos.lat;
 				int32_t lon = global_pos.lon;
@@ -845,6 +910,126 @@ static void *uorb_receiveloop(void *arg)
 				orb_copy(ORB_ID(vehicle_attitude_setpoint), spa_sub, &buf.att_sp);
 				mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, buf.att_sp.roll_tait_bryan, buf.att_sp.pitch_tait_bryan, buf.att_sp.yaw_tait_bryan, buf.att_sp.thrust);
 			}
+
+			/* --- ACTUATOR OUTPUTS 0 --- */
+			if (fds[ifds++].revents & POLLIN) {
+				/* copy actuator data into local buffer */
+				orb_copy(ORB_ID(actuator_outputs_0), subs->act_0_sub, &buf.act_outputs);
+				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					0 /* port 0 */,
+					buf.act_outputs.output[0],
+					buf.act_outputs.output[1],
+					buf.act_outputs.output[2],
+					buf.act_outputs.output[3],
+					buf.act_outputs.output[4],
+					buf.act_outputs.output[5],
+					buf.act_outputs.output[6],
+					buf.act_outputs.output[7]);
+				// if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
+				// 	mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+				// 	1 /* port 1 */,
+				// 	buf.act_outputs.output[ 8],
+				// 	buf.act_outputs.output[ 9],
+				// 	buf.act_outputs.output[10],
+				// 	buf.act_outputs.output[11],
+				// 	buf.act_outputs.output[12],
+				// 	buf.act_outputs.output[13],
+				// 	buf.act_outputs.output[14],
+				// 	buf.act_outputs.output[15]);
+				// }
+			}
+
+			/* --- ACTUATOR OUTPUTS 1 --- */
+			if (fds[ifds++].revents & POLLIN) {
+				/* copy actuator data into local buffer */
+				orb_copy(ORB_ID(actuator_outputs_1), subs->act_1_sub, &buf.act_outputs);
+				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 2 : 1 /* port 2 or 1*/,
+					buf.act_outputs.output[0],
+					buf.act_outputs.output[1],
+					buf.act_outputs.output[2],
+					buf.act_outputs.output[3],
+					buf.act_outputs.output[4],
+					buf.act_outputs.output[5],
+					buf.act_outputs.output[6],
+					buf.act_outputs.output[7]);
+				if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
+					mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					3 /* port 3 */,
+					buf.act_outputs.output[ 8],
+					buf.act_outputs.output[ 9],
+					buf.act_outputs.output[10],
+					buf.act_outputs.output[11],
+					buf.act_outputs.output[12],
+					buf.act_outputs.output[13],
+					buf.act_outputs.output[14],
+					buf.act_outputs.output[15]);
+				}
+			}
+
+			/* --- ACTUATOR OUTPUTS 2 --- */
+			if (fds[ifds++].revents & POLLIN) {
+				/* copy actuator data into local buffer */
+				orb_copy(ORB_ID(actuator_outputs_2), subs->act_2_sub, &buf.act_outputs);
+				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 4 : 2 /* port 4 or 2 */,
+					buf.act_outputs.output[0],
+					buf.act_outputs.output[1],
+					buf.act_outputs.output[2],
+					buf.act_outputs.output[3],
+					buf.act_outputs.output[4],
+					buf.act_outputs.output[5],
+					buf.act_outputs.output[6],
+					buf.act_outputs.output[7]);
+				if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
+					mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					5 /* port 5 */,
+					buf.act_outputs.output[ 8],
+					buf.act_outputs.output[ 9],
+					buf.act_outputs.output[10],
+					buf.act_outputs.output[11],
+					buf.act_outputs.output[12],
+					buf.act_outputs.output[13],
+					buf.act_outputs.output[14],
+					buf.act_outputs.output[15]);
+				}
+			}
+
+			/* --- ACTUATOR OUTPUTS 3 --- */
+			if (fds[ifds++].revents & POLLIN) {
+				/* copy actuator data into local buffer */
+				orb_copy(ORB_ID(actuator_outputs_3), subs->act_3_sub, &buf.act_outputs);
+				mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					(NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 6 : 3 /* port 6 or 3 */,
+					buf.act_outputs.output[0],
+					buf.act_outputs.output[1],
+					buf.act_outputs.output[2],
+					buf.act_outputs.output[3],
+					buf.act_outputs.output[4],
+					buf.act_outputs.output[5],
+					buf.act_outputs.output[6],
+					buf.act_outputs.output[7]);
+				if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) {
+					mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(),
+					7 /* port 7 */,
+					buf.act_outputs.output[ 8],
+					buf.act_outputs.output[ 9],
+					buf.act_outputs.output[10],
+					buf.act_outputs.output[11],
+					buf.act_outputs.output[12],
+					buf.act_outputs.output[13],
+					buf.act_outputs.output[14],
+					buf.act_outputs.output[15]);
+				}
+			}
+
+			/* --- MAPPED MANUAL CONTROL INPUTS --- */
+			if (fds[ifds++].revents & POLLIN) {
+				/* copy local position data into local buffer */
+				orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control);
+				mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll,
+					buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 1, 1, 1, 1);
+			}
 		}
 	}
 
@@ -891,7 +1076,8 @@ void handleMessage(mavlink_message_t *msg)
 		mavlink_command_long_t cmd_mavlink;
 		mavlink_msg_command_long_decode(msg, &cmd_mavlink);
 
-		if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
+		if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
+			|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
 			//check for MAVLINK terminate command
 			if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
 				/* This is the link shutdown command, terminate mavlink */ //TODO: check what happens with global_data buffers that are read by the mavlink app
@@ -1299,8 +1485,8 @@ int mavlink_thread_main(int argc, char *argv[])
 
 	/* topics to subscribe globally */
 	/* subscribe to ORB for global position */
-	global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	orb_set_interval(global_pos_sub, 1000);	/* 1Hz active updates */
+	orb_set_interval(mavlink_subs.global_pos_sub, 1000);	/* 1Hz active updates */
 	/* subscribe to ORB for local position */
 	local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
 	orb_set_interval(local_pos_sub, 1000);	/* 1Hz active updates */
@@ -1315,7 +1501,7 @@ int mavlink_thread_main(int argc, char *argv[])
 	pthread_attr_init(&uorb_attr);
 	/* Set stack size, needs more than 4000 bytes */
 	pthread_attr_setstacksize(&uorb_attr, 4096);
-	pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, NULL);
+	pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, &mavlink_subs);
 
 	/* initialize waypoint manager */
 	mavlink_wpm_init(wpm);
@@ -1323,27 +1509,52 @@ int mavlink_thread_main(int argc, char *argv[])
 	uint16_t counter = 0;
 	int lowspeed_counter = 0;
 
+	/* make sure all threads have registered their subscriptions */
+	while (!mavlink_subs.initialized) {
+		usleep(500);
+	}
+
 	/* all subscriptions are now active, set up initial guess about rate limits */
 	if (baudrate >= 921600) {
 		/* 500 Hz / 2 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 2);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 2);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 2);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 2);
+		/* 200 Hz / 5 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
+		/* 5 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
 	} else if (baudrate >= 460800) {
 		/* 250 Hz / 4 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 5);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 5);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
+		/* 50 Hz / 20 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 20);
+		/* 2 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
 	} else if (baudrate >= 115200) {
 		/* 50 Hz / 20 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 50);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 50);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
+		/* 10 Hz / 100 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100);
+		/* 1 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
 	} else if (baudrate >= 57600) {
 		/* 10 Hz / 100 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 100);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 100);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 100);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100);
+		/* 5 Hz / 200 ms */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
+		/* 0.2 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 5000);
 	} else {
 		/* very low baud rate, limit to 1 Hz / 1000 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1000);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 1000);
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
+		/* 0.5 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
+		/* 0.1 Hz */
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
 	}
 
 	while (!thread_should_exit) {
@@ -1351,7 +1562,7 @@ int mavlink_thread_main(int argc, char *argv[])
 		if (mavlink_exit_requested) break;
 
 		/* get local and global position */
-		orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
+		orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
 		orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
 
 		/* check if waypoint has been reached against the last positions */

apps/multirotor_att_control/multirotor_att_control_main.c

@@ -78,13 +78,11 @@ static enum {
 
 static bool thread_should_exit;
 static int mc_task;
+static bool motor_test_mode = false;
 
 static int
 mc_thread_main(int argc, char *argv[])
 {
-	bool motor_test_mode = false;
-
-	/* structures */
 	/* declare and safely initialize all structs */
 	struct vehicle_status_s state;
 	memset(&state, 0, sizeof(state));
@@ -109,7 +107,7 @@ mc_thread_main(int argc, char *argv[])
 	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	//int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	// int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
 
 	/* rate-limit the attitude subscription to 200Hz to pace our loop */
@@ -120,8 +118,9 @@ mc_thread_main(int argc, char *argv[])
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
 		actuators.control[i] = 0.0f;
 	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
-	armed.armed = true;
+	armed.armed = false;
 	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 
 	/* register the perf counter */
 	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control");
@@ -136,6 +135,8 @@ mc_thread_main(int argc, char *argv[])
 
 		perf_begin(mc_loop_perf);
 
+		/* get a local copy of system state */
+		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
 		/* get a local copy of manual setpoint */
 		orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
 		/* get a local copy of attitude */
@@ -150,7 +151,7 @@ mc_thread_main(int argc, char *argv[])
 			att_sp.roll_body = 0.0f;
 			att_sp.pitch_body = 0.0f;
 			att_sp.yaw_body = 0.0f;
-			att_sp.thrust = 0.3f;
+			att_sp.thrust = 0.1f;
 		} else {
 			if (state.state_machine == SYSTEM_STATE_MANUAL ||
 				state.state_machine == SYSTEM_STATE_GROUND_READY ||
@@ -159,28 +160,30 @@ mc_thread_main(int argc, char *argv[])
 				state.state_machine == SYSTEM_STATE_MISSION_ABORT ||
 				state.state_machine == SYSTEM_STATE_EMCY_LANDING) {
 				att_sp.thrust = manual.throttle;
+				armed.armed = true;
 
 			} else if (state.state_machine == SYSTEM_STATE_EMCY_CUTOFF) {
 				/* immediately cut off motors */
-				att_sp.thrust = 0.0f;
+				armed.armed = false;
 
 			} else {
 				/* limit motor throttle to zero for an unknown mode */
-				att_sp.thrust = 0.0f;
+				armed.armed = false;
 			}
 			
 		}
 
 		multirotor_control_attitude(&att_sp, &att, &state, &actuator_controls, motor_test_mode);
-		//ardrone_mixing_and_output(ardrone_write, &actuator_controls, motor_test_mode);
 
 		/* publish the result */
+		orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+		orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 
 		perf_end(mc_loop_perf);
 	}
 
-	printf("[multirotor att control] stopping.\n");
+	printf("[multirotor att control] stopping, disarming motors.\n");
 
 	/* kill all outputs */
 	armed.armed = false;
@@ -208,8 +211,9 @@ usage(const char *reason)
 {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: multirotor_att_control [-m <mode>] {start|stop}\n");
+	fprintf(stderr, "usage: multirotor_att_control [-m <mode>] [-t] {start|stop}\n");
 	fprintf(stderr, "    <mode> is 'rates' or 'attitude'\n");
+	fprintf(stderr, "    -t enables motor test mode with 10%% thrust\n");
 	exit(1);
 }
 
@@ -220,7 +224,7 @@ int multirotor_att_control_main(int argc, char *argv[])
 	control_mode = CONTROL_MODE_RATES;
 	unsigned int optioncount = 0;
 
-	while ((ch = getopt(argc, argv, "m:")) != EOF) {
+	while ((ch = getopt(argc, argv, "tm:")) != EOF) {
 		switch (ch) {
 		case 'm':
 			if (!strcmp(optarg, "rates")) {
@@ -231,24 +235,33 @@ int multirotor_att_control_main(int argc, char *argv[])
 				usage("unrecognized -m value");
 			}
 			optioncount += 2;
+			break;
+		case 't':
+			motor_test_mode = true;
+			optioncount += 1;
+			break;
+		case ':':
+			usage("missing parameter");
+			break;
 		default:
+			fprintf(stderr, "option: -%c\n", ch);
 			usage("unrecognized option");
 		}
 	}
 	argc -= optioncount;
-	argv += optioncount;
+	//argv += optioncount;
 
 	if (argc < 1)
 		usage("missing command");
 
-	if (!strcmp(argv[1], "start")) {
+	if (!strcmp(argv[1+optioncount], "start")) {
 
 		thread_should_exit = false;
 		mc_task = task_create("multirotor_att_control", SCHED_PRIORITY_MAX - 15, 2048, mc_thread_main, NULL);
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "stop")) {
+	if (!strcmp(argv[1+optioncount], "stop")) {
 		thread_should_exit = true;
 		exit(0);
 	}

apps/multirotor_att_control/multirotor_attitude_control.c

@@ -239,4 +239,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 	actuators->control[1] = pitch_control;
 	actuators->control[2] = yaw_rate_control;
 	actuators->control[3] = motor_thrust;
+
+	motor_skip_counter++;
 }

apps/px4/fmu/fmu.cpp

@@ -212,7 +212,7 @@ FMUServo::task_main()
 	orb_set_interval(_t_armed, 100);		/* 10Hz update rate */
 
 	/* advertise the mixed control outputs */
-	struct actuator_output_s outputs;
+	struct actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
 	_t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
 

apps/sensors/sensors.c

@@ -832,7 +832,7 @@ int sensors_thread_main(int argc, char *argv[])
 					if (manual_control.yaw >  1.0f) manual_control.yaw =  1.0f;
 					
 					/* throttle input */
-					manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled/2.0f;
+					manual_control.throttle = (rc.chan[rc.function[THROTTLE]].scaled+1.0f)/2.0f;
 					if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
 					if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;
 

apps/uORB/objects_common.cpp

@@ -116,7 +116,7 @@ ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);
 
 #include "topics/actuator_outputs.h"
-ORB_DEFINE(actuator_outputs_0, struct actuator_output_s);
+ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
-ORB_DEFINE(actuator_outputs_1, struct actuator_output_s);
+ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
-ORB_DEFINE(actuator_outputs_2, struct actuator_output_s);
+ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
-ORB_DEFINE(actuator_outputs_3, struct actuator_output_s);
+ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);

apps/uORB/topics/actuator_outputs.h

@@ -52,7 +52,7 @@
 #define NUM_ACTUATOR_OUTPUTS		16
 #define NUM_ACTUATOR_OUTPUT_GROUPS	4	/**< for sanity checking */
 
-struct actuator_output_s {
+struct actuator_outputs_s {
 	float	output[NUM_ACTUATOR_OUTPUTS];
 };