Merge remote-tracking branch 'upstream/master' into io

Fixed Conflicts:
	apps/multirotor_att_control/multirotor_att_control_main.c

rc loss failsafe throttle tested

2 files changed, 22 insertions, 3 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 2b59f709a..c3e825a86 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -63,7 +63,6 @@
 #include <string.h>
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_hrt.h>
 #include <drivers/drv_tone_alarm.h>
 #include "state_machine_helper.h"
 #include "systemlib/systemlib.h"
@@ -269,6 +268,7 @@ void tune_confirm(void) {
 
 void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 {
+
 	/* set to mag calibration mode */
 	status->flag_preflight_mag_calibration = true;
 	state_machine_publish(status_pub, status, mavlink_fd);
@@ -325,7 +325,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	uint64_t axis_deadline = hrt_absolute_time();
 	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
 
-	const char axislabels[3] = { 'X', 'Z', 'Y'};
+	const char axislabels[3] = { 'X', 'Y', 'Z'};
 	int axis_index = -1;
 
 	float *x = (float*)malloc(sizeof(float) * calibration_maxcount);
@@ -471,6 +471,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		int save_ret = param_save_default();
 		if(save_ret != 0) {
 			warn("WARNING: auto-save of params to storage failed");
+			mavlink_log_info(mavlink_fd, "[cmd] FAILED storing calibration");
 		}
 
 		printf("[mag cal]\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
@@ -1132,7 +1133,7 @@ int commander_main(int argc, char *argv[])
 		daemon_task = task_spawn("commander",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 50,
-					 4096,
+					 4000,
 					 commander_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index 9c7db8fca..bf50ebad2 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -213,6 +213,24 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 	/* publish the new state */
 	current_status->counter++;
 	current_status->timestamp = hrt_absolute_time();
+
+	/* assemble state vector based on flag values */
+	if (current_status->flag_control_rates_enabled) {
+		current_status->onboard_control_sensors_present |= 0x400;
+	} else {
+		current_status->onboard_control_sensors_present &= ~0x400;
+	}
+	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
+	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
+	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
+	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
+
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0;
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
+
 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
 	printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
 }