Merged master

18 files changed, 1257 insertions, 846 deletions

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 7180048ff..7a4e7a766 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -131,6 +130,7 @@
 #include <fcntl.h>
 #include <sys/prctl.h>
 #include <math.h>
+#include <float.h>
 #include <mathlib/mathlib.h>
 #include <string.h>
 #include <drivers/drv_hrt.h>
@@ -158,6 +158,8 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 
 int do_accel_calibration(int mavlink_fd)
 {
+	int fd;
+
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 
 	struct accel_scale accel_scale = {
@@ -172,7 +174,7 @@ int do_accel_calibration(int mavlink_fd)
 	int res = OK;
 
 	/* reset all offsets to zero and all scales to one */
-	int fd = open(ACCEL_DEVICE_PATH, 0);
+	fd = open(ACCEL_DEVICE_PATH, 0);
 	res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
 	close(fd);
 
@@ -223,7 +225,7 @@ int do_accel_calibration(int mavlink_fd)
 
 	if (res == OK) {
 		/* apply new scaling and offsets */
-		int fd = open(ACCEL_DEVICE_PATH, 0);
+		fd = open(ACCEL_DEVICE_PATH, 0);
 		res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
 		close(fd);
 
@@ -524,7 +526,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
 		    src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
 		    src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
 
-	if (det == 0.0f) {
+	if (fabsf(det) < FLT_EPSILON) {
 		return ERROR;        // Singular matrix
 	}
 
diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h
index 1cf9c0977..6b7e16d44 100644
--- a/src/modules/commander/accelerometer_calibration.h
+++ b/src/modules/commander/accelerometer_calibration.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 5d21d89d0..0e58c68b6 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -51,6 +51,7 @@
 #include <mavlink/mavlink_log.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
+#include <systemlib/airspeed.h>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -64,19 +65,17 @@ int do_airspeed_calibration(int mavlink_fd)
 {
 	/* give directions */
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
-	mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
 
-	const int calibration_count = 2000;
+	const unsigned calibration_count = 2000;
 
 	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	struct differential_pressure_s diff_pres;
 
-	int calibration_counter = 0;
 	float diff_pres_offset = 0.0f;
 
 	/* Reset sensor parameters */
 	struct airspeed_scale airscale = {
-		0.0f,
+		diff_pres_offset,
 		1.0f,
 	};
 
@@ -95,12 +94,29 @@ int do_airspeed_calibration(int mavlink_fd)
 	}
 
 	if (!paramreset_successful) {
-		warn("FAILED to set scale / offsets for airspeed");
-		mavlink_log_critical(mavlink_fd, "dpress reset failed");
-		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
-		return ERROR;
+
+		/* only warn if analog scaling is zero */
+		float analog_scaling = 0.0f;
+		param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
+		if (fabsf(analog_scaling) < 0.1f) {
+			mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
+			close(diff_pres_sub);
+			return ERROR;
+		}
+
+		/* set scaling offset parameter */
+		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+			close(diff_pres_sub);
+			return ERROR;
+		}
 	}
 
+	unsigned calibration_counter = 0;
+
+	mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
+	usleep(500 * 1000);
+
 	while (calibration_counter < calibration_count) {
 
 		/* wait blocking for new data */
@@ -112,11 +128,12 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		if (poll_ret) {
 			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+
 			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
 			calibration_counter++;
 
 			if (calibration_counter % (calibration_count / 20) == 0) {
-				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
+				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count);
 			}
 
 		} else if (poll_ret == 0) {
@@ -131,6 +148,16 @@ int do_airspeed_calibration(int mavlink_fd)
 
 	if (isfinite(diff_pres_offset)) {
 
+		int  fd_scale = open(AIRSPEED_DEVICE_PATH, 0);
+		airscale.offset_pa = diff_pres_offset;
+		if (fd_scale > 0) {
+			if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
+				mavlink_log_critical(mavlink_fd, "airspeed offset update failed");
+			}
+
+			close(fd_scale);
+		}
+
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
 			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 			close(diff_pres_sub);
@@ -147,14 +174,91 @@ int do_airspeed_calibration(int mavlink_fd)
 			return ERROR;
 		}
 
-		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
-		tune_neutral(true);
-		close(diff_pres_sub);
-		return OK;
-
 	} else {
 		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 		close(diff_pres_sub);
 		return ERROR;
 	}
+
+	mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset);
+
+	/* wait 500 ms to ensure parameter propagated through the system */
+	usleep(500 * 1000);
+
+	calibration_counter = 0;
+	const unsigned maxcount = 3000;
+
+	/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
+	while (calibration_counter < maxcount) {
+
+		/* wait blocking for new data */
+		struct pollfd fds[1];
+		fds[0].fd = diff_pres_sub;
+		fds[0].events = POLLIN;
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret) {
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+
+			calibration_counter++;
+
+			if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
+				if (calibration_counter % 100 == 0) {
+					mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)",
+					(int)diff_pres.differential_pressure_raw_pa);
+				}
+				continue;
+			}
+
+			/* do not allow negative values */
+			if (diff_pres.differential_pressure_raw_pa < 0.0f) {
+				mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)",
+					(int)diff_pres.differential_pressure_raw_pa);
+				mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa);
+				close(diff_pres_sub);
+
+				/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
+
+				diff_pres_offset = 0.0f;
+				if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
+					mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+					close(diff_pres_sub);
+					return ERROR;
+				}
+
+				/* save */
+				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0);
+				(void)param_save_default();
+
+				close(diff_pres_sub);
+
+				mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+				return ERROR;
+			} else {
+				mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)",
+					(int)diff_pres.differential_pressure_raw_pa);
+				break;
+			}
+
+		} else if (poll_ret == 0) {
+			/* any poll failure for 1s is a reason to abort */
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+			close(diff_pres_sub);
+			return ERROR;
+		}
+	}
+
+	if (calibration_counter == maxcount) {
+		mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		close(diff_pres_sub);
+		return ERROR;
+	}
+
+	mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
+
+	mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+	tune_neutral(true);
+	close(diff_pres_sub);
+	return OK;
 }
diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h
index 71c701fc2..8c6b65ff1 100644
--- a/src/modules/commander/airspeed_calibration.h
+++ b/src/modules/commander/airspeed_calibration.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp
index 3123c4087..da98644b4 100644
--- a/src/modules/commander/baro_calibration.cpp
+++ b/src/modules/commander/baro_calibration.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h
index bc42bc6cf..ce78ae700 100644
--- a/src/modules/commander/baro_calibration.h
+++ b/src/modules/commander/baro_calibration.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h
index fd8b8564d..b1e209efc 100644
--- a/src/modules/commander/calibration_messages.h
+++ b/src/modules/commander/calibration_messages.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index be38ea104..5796204bf 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,6 +39,7 @@
  */
 
 #include <math.h>
+#include <float.h>
 
 #include "calibration_routines.h"
 
@@ -170,7 +170,7 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 	float aA, aB, aC, nA, nB, nC, dA, dB, dC;
 
 	//Iterate N times, ignore stop condition.
-	int n = 0;
+	unsigned int n = 0;
 
 	while (n < max_iterations) {
 		n++;
@@ -179,9 +179,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 		aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
 		aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
 		aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
-		aA = (aA == 0.0f) ? 1.0f : aA;
-		aB = (aB == 0.0f) ? 1.0f : aB;
-		aC = (aC == 0.0f) ? 1.0f : aC;
+		aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA;
+		aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB;
+		aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC;
 
 		//Compute next iteration
 		nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h
index e3e7fbafd..3c8e49ee9 100644
--- a/src/modules/commander/calibration_routines.h
+++ b/src/modules/commander/calibration_routines.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 65922b2a5..0c4d48dea 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1,11 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,8 +33,13 @@
 
 /**
  * @file commander.cpp
- * Main system state machine implementation.
+ * Main fail-safe handling.
  *
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
@@ -52,6 +52,7 @@
 #include <fcntl.h>
 #include <errno.h>
 #include <systemlib/err.h>
+#include <systemlib/circuit_breaker.h>
 #include <debug.h>
 #include <sys/prctl.h>
 #include <sys/stat.h>
@@ -76,6 +77,10 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/safety.h>
+#include <uORB/topics/system_power.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+#include <uORB/topics/telemetry_status.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -87,6 +92,8 @@
 #include <systemlib/err.h>
 #include <systemlib/cpuload.h>
 #include <systemlib/rc_check.h>
+#include <systemlib/state_table.h>
+#include <dataman/dataman.h>
 
 #include "px4_custom_mode.h"
 #include "commander_helper.h"
@@ -120,6 +127,8 @@ extern struct system_load_s system_load;
 #define POSITION_TIMEOUT		(600 * 1000)		/**< consider the local or global position estimate invalid after 600ms */
 #define FAILSAFE_DEFAULT_TIMEOUT	(3 * 1000 * 1000)	/**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
 #define RC_TIMEOUT			500000
+#define DL_TIMEOUT			5 * 1000* 1000
+#define OFFBOARD_TIMEOUT		500000
 #define DIFFPRESS_TIMEOUT		2000000
 
 #define PRINT_INTERVAL	5000000
@@ -160,6 +169,7 @@ static struct vehicle_status_s status;
 static struct actuator_armed_s armed;
 static struct safety_s safety;
 static struct vehicle_control_mode_s control_mode;
+static struct offboard_control_setpoint_s sp_offboard;
 
 /* tasks waiting for low prio thread */
 typedef enum {
@@ -278,15 +288,22 @@ int commander_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("\tcommander is running");
-			print_status();
+	/* commands needing the app to run below */
+	if (!thread_running) {
+		warnx("\tcommander not started");
+		exit(1);
+	}
 
-		} else {
-			warnx("\tcommander not started");
-		}
+	if (!strcmp(argv[1], "status")) {
+		print_status();
+		exit(0);
+	}
 
+	if (!strcmp(argv[1], "check")) {
+		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+		int checkres = prearm_check(&status, mavlink_fd_local);
+		close(mavlink_fd_local);
+		warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED");
 		exit(0);
 	}
 
@@ -295,7 +312,7 @@ int commander_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "2")) {
+	if (!strcmp(argv[1], "disarm")) {
 		arm_disarm(false, mavlink_fd, "command line");
 		exit(0);
 	}
@@ -316,6 +333,7 @@ void usage(const char *reason)
 
 void print_status()
 {
+	warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE");
 	warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
 
 	/* read all relevant states */
@@ -367,16 +385,16 @@ void print_status()
 
 static orb_advert_t status_pub;
 
-transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy)
+transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char *armedBy)
 {
 	transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
 	// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
 	// output appropriate error messages if the state cannot transition.
-	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
+	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local);
 
 	if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
-		mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
+		mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
 
 	} else if (arming_res == TRANSITION_DENIED) {
 		tune_negative(true);
@@ -385,133 +403,113 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed
 	return arming_res;
 }
 
-bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
+bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
+	struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
+	struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
 {
-	/* result of the command */
-	enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-	bool ret = false;
-
 	/* only handle commands that are meant to be handled by this system and component */
-	if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
+	if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components
 		return false;
 	}
 
-	/* only handle high-priority commands here */
+	/* result of the command */
+	enum VEHICLE_CMD_RESULT cmd_result = VEHICLE_CMD_RESULT_UNSUPPORTED;
 
 	/* request to set different system mode */
 	switch (cmd->command) {
 	case VEHICLE_CMD_DO_SET_MODE: {
-			uint8_t base_mode = (uint8_t) cmd->param1;
-			uint8_t custom_main_mode = (uint8_t) cmd->param2;
-			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
-
-			/* set HIL state */
-			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
-			int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd);
+			uint8_t base_mode = (uint8_t)cmd->param1;
+			uint8_t custom_main_mode = (uint8_t)cmd->param2;
 
-			/* if HIL got enabled, reset battery status state */
-			if (hil_ret == OK && status->hil_state == HIL_STATE_ON) {
-				/* reset the arming mode to disarmed */
-				arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed);
+			transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
 
-				if (arming_res != TRANSITION_DENIED) {
-					mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
-
-				} else {
-					mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
-				}
-			}
+			transition_result_t main_ret = TRANSITION_NOT_CHANGED;
 
-			if (hil_ret == OK) {
-				ret = true;
-			}
+			/* set HIL state */
+			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
+			transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
 
 			// Transition the arming state
-			arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
-
-			if (arming_res == TRANSITION_CHANGED) {
-				ret = true;
-			}
-
-			/* set main state */
-			transition_result_t main_res = TRANSITION_DENIED;
+			arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
 
 			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
 				/* use autopilot-specific mode */
 				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
 					/* MANUAL */
-					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+					main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
 					/* ALTCTL */
-					main_res = main_state_transition(status, MAIN_STATE_ALTCTL);
+					main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
 					/* POSCTL */
-					main_res = main_state_transition(status, MAIN_STATE_POSCTL);
+					main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */
-					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+					main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
 					/* ACRO */
-					main_res = main_state_transition(status, MAIN_STATE_ACRO);
+					main_ret = main_state_transition(status_local, MAIN_STATE_ACRO);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
+					/* OFFBOARD */
+					main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
 				}
 
 			} else {
 				/* use base mode */
 				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
 					/* AUTO */
-					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+					main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
 
 				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
 					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
 						/* POSCTL */
-						main_res = main_state_transition(status, MAIN_STATE_POSCTL);
+						main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
 
 					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
 						/* MANUAL */
-						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+						main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL);
 					}
 				}
 			}
 
-			if (main_res == TRANSITION_CHANGED) {
-				ret = true;
-			}
-
-			if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+			if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) {
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
-
-			break;
 		}
+		break;
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-			// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
-			// We use an float epsilon delta to test float equality.
-			if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
-				mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
+			// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
+			// for logic state parameters
+
+			if (static_cast<int>(cmd->param1 + 0.5f) != 0 && static_cast<int>(cmd->param1 + 0.5f) != 1) {
+				mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1);
 
 			} else {
 
+				bool cmd_arms = (static_cast<int>(cmd->param1 + 0.5f) == 1);
+
 				// Flick to inair restore first if this comes from an onboard system
-				if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) {
-					status->arming_state = ARMING_STATE_IN_AIR_RESTORE;
+				if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) {
+					status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE;
 				}
 
-				transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
+				transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
 
 				if (arming_res == TRANSITION_DENIED) {
-					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd");
+					cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 
 				} else {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+					cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 				}
 			}
 		}
@@ -519,62 +517,70 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 	case VEHICLE_CMD_OVERRIDE_GOTO: {
 			// TODO listen vehicle_command topic directly from navigator (?)
-			unsigned int mav_goto = cmd->param1;
+
+			// Increase by 0.5f and rely on the integer cast
+			// implicit floor(). This is the *safest* way to
+			// convert from floats representing small ints to actual ints.
+			unsigned int mav_goto = (cmd->param1 + 0.5f);
 
 			if (mav_goto == 0) {	// MAV_GOTO_DO_HOLD
-				status->set_nav_state = NAV_STATE_LOITER;
-				status->set_nav_state_timestamp = hrt_absolute_time();
-				mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-				ret = true;
+				status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+				mavlink_log_critical(mavlink_fd, "Pause mission cmd");
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
-				status->set_nav_state = NAV_STATE_MISSION;
-				status->set_nav_state_timestamp = hrt_absolute_time();
-				mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-				ret = true;
+				status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+				mavlink_log_critical(mavlink_fd, "Continue mission cmd");
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
-				mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
+				mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
+					(double)cmd->param1,
+					(double)cmd->param2,
+					(double)cmd->param3,
+					(double)cmd->param4,
+					(double)cmd->param5,
+					(double)cmd->param6,
+					(double)cmd->param7);
 			}
 		}
 		break;
 
+#if 0
 	/* Flight termination */
 	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
 
 			//XXX: to enable the parachute, a param needs to be set
 			//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
-			if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) {
+			if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
 				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-				ret = true;
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
 				/* reject parachute depoyment not armed */
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 
 		}
 		break;
+#endif
 
 	case VEHICLE_CMD_DO_SET_HOME: {
 			bool use_current = cmd->param1 > 0.5f;
 
 			if (use_current) {
 				/* use current position */
-				if (status->condition_global_position_valid) {
+				if (status_local->condition_global_position_valid) {
 					home->lat = global_pos->lat;
 					home->lon = global_pos->lon;
 					home->alt = global_pos->alt;
 
 					home->timestamp = hrt_absolute_time();
 
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+					cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
-					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 				}
 
 			} else {
@@ -585,10 +591,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 				home->timestamp = hrt_absolute_time();
 
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 			}
 
-			if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
+			if (cmd_result == VEHICLE_CMD_RESULT_ACCEPTED) {
 				warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt);
 				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt);
 
@@ -601,7 +607,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				}
 
 				/* mark home position as set */
-				status->condition_home_position_valid = true;
+				status_local->condition_home_position_valid = true;
 			}
 		}
 		break;
@@ -620,17 +626,18 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		break;
 	}
 
-	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+	if (cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* already warned about unsupported commands in "default" case */
-		answer_command(*cmd, result);
+		answer_command(*cmd, cmd_result);
 	}
 
 	/* send any requested ACKs */
-	if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+	if (cmd->confirmation > 0 && cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* send acknowledge command */
 		// XXX TODO
 	}
 
+	return true;
 }
 
 int commander_thread_main(int argc, char *argv[])
@@ -647,42 +654,54 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_component_id = param_find("MAV_COMP_ID");
 	param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
 	param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
+	param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN");
 
 	/* welcome user */
 	warnx("starting");
 
-	char *main_states_str[MAIN_STATE_MAX];
-	main_states_str[0] = "MANUAL";
-	main_states_str[1] = "ALTCTL";
-	main_states_str[2] = "POSCTL";
-	main_states_str[3] = "AUTO";
-	main_states_str[4] = "ACRO";
-
-	char *arming_states_str[ARMING_STATE_MAX];
-	arming_states_str[0] = "INIT";
-	arming_states_str[1] = "STANDBY";
-	arming_states_str[2] = "ARMED";
-	arming_states_str[3] = "ARMED_ERROR";
-	arming_states_str[4] = "STANDBY_ERROR";
-	arming_states_str[5] = "REBOOT";
-	arming_states_str[6] = "IN_AIR_RESTORE";
-
-	char *failsafe_states_str[FAILSAFE_STATE_MAX];
-	failsafe_states_str[0] = "NORMAL";
-	failsafe_states_str[1] = "RTL";
-	failsafe_states_str[2] = "LAND";
-	failsafe_states_str[3] = "TERMINATION";
+	const char *main_states_str[MAIN_STATE_MAX];
+	main_states_str[MAIN_STATE_MANUAL]			= "MANUAL";
+	main_states_str[MAIN_STATE_ALTCTL]			= "ALTCTL";
+	main_states_str[MAIN_STATE_POSCTL]			= "POSCTL";
+	main_states_str[MAIN_STATE_AUTO_MISSION]		= "AUTO_MISSION";
+	main_states_str[MAIN_STATE_AUTO_LOITER]			= "AUTO_LOITER";
+	main_states_str[MAIN_STATE_AUTO_RTL]			= "AUTO_RTL";
+	main_states_str[MAIN_STATE_ACRO]			= "ACRO";
+	main_states_str[MAIN_STATE_OFFBOARD]			= "OFFBOARD";
+
+	const char *arming_states_str[ARMING_STATE_MAX];
+	arming_states_str[ARMING_STATE_INIT]			= "INIT";
+	arming_states_str[ARMING_STATE_STANDBY]			= "STANDBY";
+	arming_states_str[ARMING_STATE_ARMED]			= "ARMED";
+	arming_states_str[ARMING_STATE_ARMED_ERROR]		= "ARMED_ERROR";
+	arming_states_str[ARMING_STATE_STANDBY_ERROR]		= "STANDBY_ERROR";
+	arming_states_str[ARMING_STATE_REBOOT]			= "REBOOT";
+	arming_states_str[ARMING_STATE_IN_AIR_RESTORE]		= "IN_AIR_RESTORE";
+
+	const char *nav_states_str[NAVIGATION_STATE_MAX];
+	nav_states_str[NAVIGATION_STATE_MANUAL]			= "MANUAL";
+	nav_states_str[NAVIGATION_STATE_ALTCTL]			= "ALTCTL";
+	nav_states_str[NAVIGATION_STATE_POSCTL]			= "POSCTL";
+	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
+	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]		= "AUTO_LOITER";
+	nav_states_str[NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
+	nav_states_str[NAVIGATION_STATE_AUTO_RTGS]		= "AUTO_RTGS";
+	nav_states_str[NAVIGATION_STATE_ACRO]			= "ACRO";
+	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
+	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
+	nav_states_str[NAVIGATION_STATE_TERMINATION]		= "TERMINATION";
+	nav_states_str[NAVIGATION_STATE_OFFBOARD]		= "OFFBOARD";
 
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
 
 	/* initialize */
 	if (led_init() != 0) {
-		warnx("ERROR: Failed to initialize leds");
+		warnx("ERROR: LED INIT FAIL");
 	}
 
 	if (buzzer_init() != OK) {
-		warnx("ERROR: Failed to initialize buzzer");
+		warnx("ERROR: BUZZER INIT FAIL");
 	}
 
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
@@ -693,11 +712,10 @@ int commander_thread_main(int argc, char *argv[])
 	// We want to accept RC inputs as default
 	status.rc_input_blocked = false;
 	status.main_state = MAIN_STATE_MANUAL;
-	status.set_nav_state = NAV_STATE_NONE;
-	status.set_nav_state_timestamp = 0;
+	status.nav_state = NAVIGATION_STATE_MANUAL;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
-	status.failsafe_state = FAILSAFE_STATE_NORMAL;
+	status.failsafe = false;
 
 	/* neither manual nor offboard control commands have been received */
 	status.offboard_control_signal_found_once = false;
@@ -706,6 +724,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* mark all signals lost as long as they haven't been found */
 	status.rc_signal_lost = true;
 	status.offboard_control_signal_lost = true;
+	status.data_link_lost = true;
 
 	/* set battery warning flag */
 	status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
@@ -717,8 +736,19 @@ int commander_thread_main(int argc, char *argv[])
 	status.counter++;
 	status.timestamp = hrt_absolute_time();
 
+	status.condition_power_input_valid = true;
+	status.avionics_power_rail_voltage = -1.0f;
+
+	// CIRCUIT BREAKERS
+	status.circuit_breaker_engaged_power_check = false;
+
 	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+	if (status_pub < 0) {
+		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
+		warnx("exiting.");
+		exit(ERROR);
+	}
 
 	/* armed topic */
 	orb_advert_t armed_pub;
@@ -736,13 +766,29 @@ int commander_thread_main(int argc, char *argv[])
 	struct home_position_s home;
 	memset(&home, 0, sizeof(home));
 
-	if (status_pub < 0) {
-		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
-		warnx("exiting.");
-		exit(ERROR);
-	}
+	/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
+	orb_advert_t mission_pub = -1;
+	mission_s mission;
+	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
+		if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
+			warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq);
+			mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
+												mission.dataman_id, mission.count, mission.current_seq);
+		} else {
+			const char *missionfail = "reading mission state failed";
+			warnx("%s", missionfail);
+			mavlink_log_critical(mavlink_fd, missionfail);
+
+			/* initialize mission state in dataman */
+			mission.dataman_id = 0;
+			mission.count = 0;
+			mission.current_seq = 0;
+			dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
+		}
 
-	mavlink_log_info(mavlink_fd, "[cmd] started");
+		mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
+		orb_publish(ORB_ID(offboard_mission), mission_pub, &mission);
+	}
 
 	int ret;
 
@@ -769,14 +815,13 @@ int commander_thread_main(int argc, char *argv[])
 
 	hrt_abstime last_idle_time = 0;
 	hrt_abstime start_time = 0;
-	hrt_abstime last_auto_state_valid = 0;
 
 	bool status_changed = true;
 	bool param_init_forced = true;
 
 	bool updated = false;
 
-	bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
+	rc_calibration_check(mavlink_fd);
 
 	/* Subscribe to safety topic */
 	int safety_sub = orb_subscribe(ORB_ID(safety));
@@ -784,6 +829,11 @@ int commander_thread_main(int argc, char *argv[])
 	safety.safety_switch_available = false;
 	safety.safety_off = false;
 
+	/* Subscribe to mission result topic */
+	int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
+	struct mission_result_s mission_result;
+	memset(&mission_result, 0, sizeof(mission_result));
+
 	/* Subscribe to manual control data */
 	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	struct manual_control_setpoint_s sp_man;
@@ -791,9 +841,19 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* Subscribe to offboard control data */
 	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	struct offboard_control_setpoint_s sp_offboard;
 	memset(&sp_offboard, 0, sizeof(sp_offboard));
 
+	/* Subscribe to telemetry status topics */
+	int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
+	uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM];
+	bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
+
+	for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+		telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
+		telemetry_last_heartbeat[i] = 0;
+		telemetry_lost[i] = true;
+	}
+
 	/* Subscribe to global position */
 	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	struct vehicle_global_position_s global_position;
@@ -853,6 +913,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct position_setpoint_triplet_s pos_sp_triplet;
 	memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
 
+	/* Subscribe to system power */
+	int system_power_sub = orb_subscribe(ORB_ID(system_power));
+	struct system_power_s system_power;
+	memset(&system_power, 0, sizeof(system_power));
+
 	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
@@ -861,6 +926,15 @@ int commander_thread_main(int argc, char *argv[])
 
 	start_time = hrt_absolute_time();
 
+	transition_result_t arming_ret;
+
+	int32_t datalink_loss_enabled = false;
+
+	/* check which state machines for changes, clear "changed" flag */
+	bool arming_state_changed = false;
+	bool main_state_changed = false;
+	bool failsafe_old = false;
+
 	while (!thread_should_exit) {
 
 		if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
@@ -868,6 +942,9 @@ int commander_thread_main(int argc, char *argv[])
 			mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
+		arming_ret = TRANSITION_NOT_CHANGED;
+
+
 		/* update parameters */
 		orb_check(param_changed_sub, &updated);
 
@@ -898,15 +975,19 @@ int commander_thread_main(int argc, char *argv[])
 				/* check and update system / component ID */
 				param_get(_param_system_id, &(status.system_id));
 				param_get(_param_component_id, &(status.component_id));
+
+				status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
+
 				status_changed = true;
 
 				/* re-check RC calibration */
-				rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
+				rc_calibration_check(mavlink_fd);
 			}
 
 			/* navigation parameters */
 			param_get(_param_takeoff_alt, &takeoff_alt);
 			param_get(_param_enable_parachute, &parachute_enabled);
+			param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
 		}
 
 		orb_check(sp_man_sub, &updated);
@@ -921,6 +1002,41 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
 		}
 
+		if (sp_offboard.timestamp != 0 &&
+		    sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) {
+			if (status.offboard_control_signal_lost) {
+				status.offboard_control_signal_lost = false;
+				status_changed = true;
+			}
+		} else {
+			if (!status.offboard_control_signal_lost) {
+				status.offboard_control_signal_lost = true;
+				status_changed = true;
+			}
+		}
+
+		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+			orb_check(telemetry_subs[i], &updated);
+
+			if (updated) {
+				struct telemetry_status_s telemetry;
+				memset(&telemetry, 0, sizeof(telemetry));
+
+				orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry);
+
+				/* perform system checks when new telemetry link connected */
+				if (mavlink_fd &&
+					telemetry_last_heartbeat[i] == 0 &&
+					telemetry.heartbeat_time > 0 &&
+					hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) {
+
+					(void)rc_calibration_check(mavlink_fd);
+				}
+
+				telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
+			}
+		}
+
 		orb_check(sensor_sub, &updated);
 
 		if (updated) {
@@ -933,6 +1049,26 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
 		}
 
+		orb_check(system_power_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(system_power), system_power_sub, &system_power);
+
+			if (hrt_elapsed_time(&system_power.timestamp) < 200000) {
+				if (system_power.servo_valid &&
+					!system_power.brick_valid &&
+					!system_power.usb_connected) {
+					/* flying only on servo rail, this is unsafe */
+					status.condition_power_input_valid = false;
+				} else {
+					status.condition_power_input_valid = true;
+				}
+
+				/* copy avionics voltage */
+				status.avionics_power_rail_voltage = system_power.voltage5V_v;
+			}
+		}
+
 		check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
 
 		/* update safety topic */
@@ -945,8 +1081,9 @@ int commander_thread_main(int argc, char *argv[])
 			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
 				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
 
-				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
-					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
+				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) {
+					mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
+					arming_state_changed = true;
 				}
 			}
 		}
@@ -959,6 +1096,14 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
 		}
 
+		/* update local position estimate */
+		orb_check(local_position_sub, &updated);
+
+		if (updated) {
+			/* position changed */
+			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
+		}
+
 		/* update condition_global_position_valid */
 		/* hysteresis for EPH/EPV */
 		bool eph_epv_good;
@@ -992,6 +1137,10 @@ int commander_thread_main(int argc, char *argv[])
 			home.lon = global_position.lon;
 			home.alt = global_position.alt;
 
+			home.x = local_position.x;
+			home.y = local_position.y;
+			home.z = local_position.z;
+
 			warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
 			mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
 
@@ -1008,14 +1157,6 @@ int commander_thread_main(int argc, char *argv[])
 			tune_positive(true);
 		}
 
-		/* update local position estimate */
-		orb_check(local_position_sub, &updated);
-
-		if (updated) {
-			/* position changed */
-			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
-		}
-
 		/* update condition_local_position_valid and condition_local_altitude_valid */
 		/* hysteresis for EPH */
 		bool local_eph_good;
@@ -1039,28 +1180,18 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed);
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
-		static bool published_condition_landed_fw = false;
-
-		if (status.is_rotary_wing && status.condition_local_altitude_valid) {
+		if (status.condition_local_altitude_valid) {
 			if (status.condition_landed != local_position.landed) {
 				status.condition_landed = local_position.landed;
 				status_changed = true;
-				published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes
 
 				if (status.condition_landed) {
-					mavlink_log_critical(mavlink_fd, "#audio: LANDED");
+					mavlink_log_critical(mavlink_fd, "LANDED MODE");
 
 				} else {
-					mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
+					mavlink_log_critical(mavlink_fd, "IN AIR MODE");
 				}
 			}
-
-		} else {
-			if (!published_condition_landed_fw) {
-				status.condition_landed = false; // Fixedwing does not have a landing detector currently
-				published_condition_landed_fw = true;
-				status_changed = true;
-			}
 		}
 
 		/* update battery status */
@@ -1138,23 +1269,30 @@ int commander_thread_main(int argc, char *argv[])
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
 		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
 			low_battery_voltage_actions_done = true;
-			mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
+			mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
 			status_changed = true;
 
 		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			critical_battery_voltage_actions_done = true;
-			mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
+			mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (armed.armed) {
-				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
+
+				if (arming_ret == TRANSITION_CHANGED) {
+					arming_state_changed = true;
+				}
 
 			} else {
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
-			}
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
 
+				if (arming_ret == TRANSITION_CHANGED) {
+					arming_state_changed = true;
+				}
+			}
 			status_changed = true;
 		}
 
@@ -1162,11 +1300,15 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
-			// XXX check for sensors
-			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+			/* TODO: check for sensors */
+			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+
+			if (arming_ret == TRANSITION_CHANGED) {
+				arming_state_changed = true;
+			}
 
 		} else {
-			// XXX: Add emergency stuff if sensors are lost
+			/* TODO: Add emergency stuff if sensors are lost */
 		}
 
 
@@ -1185,28 +1327,29 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
 		}
 
-		/* start RC input check */
+		orb_check(mission_result_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
+		}
+
+		/* RC input check */
 		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
 				status.rc_signal_found_once = true;
-				mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
+				mavlink_log_critical(mavlink_fd, "detected RC signal first time");
 				status_changed = true;
 
 			} else {
 				if (status.rc_signal_lost) {
-					mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
+					mavlink_log_critical(mavlink_fd, "RC signal regained");
 					status_changed = true;
 				}
 			}
 
 			status.rc_signal_lost = false;
 
-			transition_result_t arming_res;	// store all transitions results here
-
-			/* arm/disarm by RC */
-			arming_res = TRANSITION_NOT_CHANGED;
-
 			/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing &&
@@ -1217,7 +1360,10 @@ int commander_thread_main(int argc, char *argv[])
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
 					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
+					if (arming_ret == TRANSITION_CHANGED) {
+						arming_state_changed = true;
+					}
 					stick_off_counter = 0;
 
 				} else {
@@ -1232,14 +1378,18 @@ int commander_thread_main(int argc, char *argv[])
 			if (status.arming_state == ARMING_STATE_STANDBY &&
 			    sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-					if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
-						print_reject_arm("#audio: NOT ARMING: Press safety switch first.");
-
-					} else if (status.main_state != MAIN_STATE_MANUAL) {
-						print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
 
+					/* we check outside of the transition function here because the requirement
+					 * for being in manual mode only applies to manual arming actions.
+					 * the system can be armed in auto if armed via the GCS.
+					 */
+					if (status.main_state != MAIN_STATE_MANUAL) {
+						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 					} else {
-						arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
+						if (arming_ret == TRANSITION_CHANGED) {
+							arming_state_changed = true;
+						}
 					}
 
 					stick_on_counter = 0;
@@ -1252,22 +1402,19 @@ int commander_thread_main(int argc, char *argv[])
 				stick_on_counter = 0;
 			}
 
-			if (arming_res == TRANSITION_CHANGED) {
+			if (arming_ret == TRANSITION_CHANGED) {
 				if (status.arming_state == ARMING_STATE_ARMED) {
-					mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
+					mavlink_log_info(mavlink_fd, "ARMED by RC");
 
 				} else {
-					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
+					mavlink_log_info(mavlink_fd, "DISARMED by RC");
 				}
+				arming_state_changed = true;
 
-			} else if (arming_res == TRANSITION_DENIED) {
+			} else if (arming_ret == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
-				mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
-			}
-
-			if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
-				/* recover from failsafe */
-				(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+				mavlink_log_critical(mavlink_fd, "arming state transition denied");
+				tune_negative(true);
 			}
 
 			/* evaluate the main state machine according to mode switches */
@@ -1276,100 +1423,52 @@ int commander_thread_main(int argc, char *argv[])
 			/* play tune on mode change only if armed, blink LED always */
 			if (main_res == TRANSITION_CHANGED) {
 				tune_positive(armed.armed);
+				main_state_changed = true;
 
 			} else if (main_res == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
-				mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
-			}
-
-			/* set navigation state */
-			/* RETURN switch, overrides MISSION switch */
-			if (sp_man.return_switch == SWITCH_POS_ON) {
-				/* switch to RTL if not already landed after RTL and home position set */
-				status.set_nav_state = NAV_STATE_RTL;
-				status.set_nav_state_timestamp = hrt_absolute_time();
-
-			} else {
-
-				/* LOITER switch */
-				if (sp_man.loiter_switch == SWITCH_POS_ON) {
-					/* stick is in LOITER position */
-					status.set_nav_state = NAV_STATE_LOITER;
-					status.set_nav_state_timestamp = hrt_absolute_time();
-
-				} else if (sp_man.loiter_switch != SWITCH_POS_NONE) {
-					/* stick is in MISSION position */
-					status.set_nav_state = NAV_STATE_MISSION;
-					status.set_nav_state_timestamp = hrt_absolute_time();
-
-				} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
-					   pos_sp_triplet.nav_state == NAV_STATE_RTL) {
-					/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
-					status.set_nav_state = NAV_STATE_MISSION;
-					status.set_nav_state_timestamp = hrt_absolute_time();
-				}
+				mavlink_log_critical(mavlink_fd, "main state transition denied");
 			}
 
 		} else {
 			if (!status.rc_signal_lost) {
-				mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
+				mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST");
 				status.rc_signal_lost = true;
 				status_changed = true;
 			}
+		}
 
-			if (armed.armed) {
-				if (status.main_state == MAIN_STATE_AUTO) {
-					/* check if AUTO mode still allowed */
-					transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO);
-
-					if (auto_res == TRANSITION_NOT_CHANGED) {
-						last_auto_state_valid = hrt_absolute_time();
-					}
-
-					/* still invalid state after the timeout interval, execute failsafe */
-					if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) {
-						/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
-						auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
-
-						if (auto_res == TRANSITION_DENIED) {
-							/* LAND not allowed, set TERMINATION state */
-							(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
-						}
-					}
-
-				} else {
-					/* failsafe for manual modes */
-					transition_result_t manual_res = TRANSITION_DENIED;
-
-					if (!status.condition_landed) {
-						/* vehicle is not landed, try to perform RTL */
-						manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
-					}
-
-					if (manual_res == TRANSITION_DENIED) {
-						/* RTL not allowed (no global position estimate) or not wanted, try LAND */
-						manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
-
-						if (manual_res == TRANSITION_DENIED) {
-							/* LAND not allowed, set TERMINATION state */
-							(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
-						}
-					}
+		/* data links check */
+		bool have_link = false;
+		for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
+			if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
+				/* handle the case where data link was regained */
+				if (telemetry_lost[i]) {
+					mavlink_log_critical(mavlink_fd, "data link %i regained", i);
+					telemetry_lost[i] = false;
 				}
+				have_link = true;
 
 			} else {
-				if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
-					/* reset failsafe when disarmed */
-					(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+				if (!telemetry_lost[i]) {
+					mavlink_log_critical(mavlink_fd, "data link %i lost", i);
+					telemetry_lost[i] = true;
 				}
 			}
 		}
 
-		// TODO remove this hack
-		/* flight termination in manual mode if assist switch is on POSCTL position */
-		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) {
-			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
-				tune_positive(armed.armed);
+		if (have_link) {
+			/* handle the case where data link was regained */
+			if (status.data_link_lost) {
+				status.data_link_lost = false;
+				status_changed = true;
+			}
+
+		} else {
+			if (!status.data_link_lost) {
+				mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
+				status.data_link_lost = true;
+				status_changed = true;
 			}
 		}
 
@@ -1386,11 +1485,6 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		/* check which state machines for changes, clear "changed" flag */
-		bool arming_state_changed = check_arming_state_changed();
-		bool main_state_changed = check_main_state_changed();
-		bool failsafe_state_changed = check_failsafe_state_changed();
-
 		hrt_abstime t1 = hrt_absolute_time();
 
 		/* print new state */
@@ -1407,6 +1501,10 @@ int commander_thread_main(int argc, char *argv[])
 				home.lon = global_position.lon;
 				home.alt = global_position.alt;
 
+				home.x = local_position.x;
+				home.y = local_position.y;
+				home.z = local_position.z;
+
 				warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
 				mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
 
@@ -1421,18 +1519,33 @@ int commander_thread_main(int argc, char *argv[])
 				/* mark home position as set */
 				status.condition_home_position_valid = true;
 			}
+			arming_state_changed = false;
 		}
 
 		was_armed = armed.armed;
 
+		/* now set navigation state according to failsafe and main state */
+		bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
+						       mission_result.finished);
+
+		// TODO handle mode changes by commands
 		if (main_state_changed) {
 			status_changed = true;
+			warnx("main state: %s", main_states_str[status.main_state]);
 			mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
+			main_state_changed = false;
+		}
+
+		if (status.failsafe != failsafe_old) {
+			status_changed = true;
+			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
+			failsafe_old = status.failsafe;
 		}
 
-		if (failsafe_state_changed) {
+		if (nav_state_changed) {
 			status_changed = true;
-			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]);
+			warnx("nav state: %s", nav_states_str[status.nav_state]);
+			mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]);
 		}
 
 		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
@@ -1458,7 +1571,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* play tune on battery critical */
 			set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
 
-		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
 			/* play tune on battery warning or failsafe */
 			set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
 
@@ -1517,6 +1630,7 @@ int commander_thread_main(int argc, char *argv[])
 	close(diff_pres_sub);
 	close(param_changed_sub);
 	close(battery_sub);
+	close(mission_pub);
 
 	thread_running = false;
 
@@ -1536,22 +1650,22 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
 }
 
 void
-control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed)
+control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed)
 {
 	/* driving rgbled */
 	if (changed) {
 		bool set_normal_color = false;
 
 		/* set mode */
-		if (status->arming_state == ARMING_STATE_ARMED) {
+		if (status_local->arming_state == ARMING_STATE_ARMED) {
 			rgbled_set_mode(RGBLED_MODE_ON);
 			set_normal_color = true;
 
-		} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
+		} else if (status_local->arming_state == ARMING_STATE_ARMED_ERROR) {
 			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 			rgbled_set_color(RGBLED_COLOR_RED);
 
-		} else if (status->arming_state == ARMING_STATE_STANDBY) {
+		} else if (status_local->arming_state == ARMING_STATE_STANDBY) {
 			rgbled_set_mode(RGBLED_MODE_BREATHE);
 			set_normal_color = true;
 
@@ -1562,12 +1676,12 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 
 		if (set_normal_color) {
 			/* set color */
-			if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) {
+			if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) {
 				rgbled_set_color(RGBLED_COLOR_AMBER);
 				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
 
 			} else {
-				if (status->condition_local_position_valid) {
+				if (status_local->condition_local_position_valid) {
 					rgbled_set_color(RGBLED_COLOR_GREEN);
 
 				} else {
@@ -1600,7 +1714,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 #endif
 
 	/* give system warnings on error LED, XXX maybe add memory usage warning too */
-	if (status->load > 0.95f) {
+	if (status_local->load > 0.95f) {
 		if (leds_counter % 2 == 0) {
 			led_toggle(LED_AMBER);
 		}
@@ -1613,11 +1727,23 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 }
 
 transition_result_t
-set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man)
+set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_setpoint_s *sp_man)
 {
 	/* set main state according to RC switches */
 	transition_result_t res = TRANSITION_DENIED;
 
+	/* offboard switch overrides main switch */
+	if (sp_man->offboard_switch == SWITCH_POS_ON) {
+		res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+		if (res == TRANSITION_DENIED) {
+			print_reject_mode(status_local, "OFFBOARD");
+
+		} else {
+			return res;
+		}
+	}
+
+	/* offboard switched off or denied, check main mode switch */
 	switch (sp_man->mode_switch) {
 	case SWITCH_POS_NONE:
 		res = TRANSITION_NOT_CHANGED;
@@ -1625,58 +1751,100 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 
 	case SWITCH_POS_OFF:		// MANUAL
 		if (sp_man->acro_switch == SWITCH_POS_ON) {
-			res = main_state_transition(status, MAIN_STATE_ACRO);
+			res = main_state_transition(status_local, MAIN_STATE_ACRO);
 
 		} else {
-			res = main_state_transition(status, MAIN_STATE_MANUAL);
+			res = main_state_transition(status_local, MAIN_STATE_MANUAL);
 		}
 		// TRANSITION_DENIED is not possible here
 		break;
 
 	case SWITCH_POS_MIDDLE:		// ASSIST
 		if (sp_man->posctl_switch == SWITCH_POS_ON) {
-			res = main_state_transition(status, MAIN_STATE_POSCTL);
+			res = main_state_transition(status_local, MAIN_STATE_POSCTL);
 
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
 			}
 
-			// else fallback to ALTCTL
-			print_reject_mode(status, "POSCTL");
+			print_reject_mode(status_local, "POSCTL");
 		}
 
-		res = main_state_transition(status, MAIN_STATE_ALTCTL);
+        // fallback to ALTCTL
+		res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this mode
 		}
 
 		if (sp_man->posctl_switch != SWITCH_POS_ON) {
-			print_reject_mode(status, "ALTCTL");
+			print_reject_mode(status_local, "ALTCTL");
 		}
 
-		// else fallback to MANUAL
-		res = main_state_transition(status, MAIN_STATE_MANUAL);
+		// fallback to MANUAL
+		res = main_state_transition(status_local, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
 	case SWITCH_POS_ON:			// AUTO
-		res = main_state_transition(status, MAIN_STATE_AUTO);
+		if (sp_man->return_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL);
 
-		if (res != TRANSITION_DENIED) {
-			break;	// changed successfully or already in this state
+			if (res != TRANSITION_DENIED) {
+				break;	// changed successfully or already in this state
+			}
+
+            print_reject_mode(status_local, "AUTO_RTL");
+
+            // fallback to LOITER if home position not set
+            res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
+
+            if (res != TRANSITION_DENIED) {
+                break;  // changed successfully or already in this state
+            }
+
+		} else if (sp_man->loiter_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
+
+			if (res != TRANSITION_DENIED) {
+				break;	// changed successfully or already in this state
+			}
+
+            print_reject_mode(status_local, "AUTO_LOITER");
+
+		} else {
+			res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
+
+			if (res != TRANSITION_DENIED) {
+				break;	// changed successfully or already in this state
+			}
+
+            print_reject_mode(status_local, "AUTO_MISSION");
+
+            // fallback to LOITER if home position not set
+            res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
+
+            if (res != TRANSITION_DENIED) {
+                break;  // changed successfully or already in this state
+            }
 		}
 
-		// else fallback to ALTCTL (POSCTL likely will not work too)
-		print_reject_mode(status, "AUTO");
-		res = main_state_transition(status, MAIN_STATE_ALTCTL);
+        // fallback to POSCTL
+        res = main_state_transition(status_local, MAIN_STATE_POSCTL);
+
+        if (res != TRANSITION_DENIED) {
+            break;  // changed successfully or already in this state
+        }
+
+		// fallback to ALTCTL
+		res = main_state_transition(status_local, MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
 		}
 
-		// else fallback to MANUAL
-		res = main_state_transition(status, MAIN_STATE_MANUAL);
+		// fallback to MANUAL
+		res = main_state_transition(status_local, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
 		break;
 
@@ -1688,47 +1856,83 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 }
 
 void
-
 set_control_mode()
 {
-	/* set vehicle_control_mode according to main state and failsafe state */
+	/* set vehicle_control_mode according to set_navigation_state */
 	control_mode.flag_armed = armed.armed;
+	/* TODO: check this */
 	control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
 	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
+	control_mode.flag_control_offboard_enabled = false;
 
-	control_mode.flag_control_termination_enabled = false;
+	switch (status.nav_state) {
+	case NAVIGATION_STATE_MANUAL:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = status.is_rotary_wing;
+		control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		break;
 
-	/* set this flag when navigator should act */
-	bool navigator_enabled = false;
+	case NAVIGATION_STATE_ACRO:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		break;
 
-	switch (status.failsafe_state) {
-	case FAILSAFE_STATE_NORMAL:
-		switch (status.main_state) {
-		case MAIN_STATE_MANUAL:
-			control_mode.flag_control_manual_enabled = true;
-			control_mode.flag_control_auto_enabled = false;
-			control_mode.flag_control_rates_enabled = status.is_rotary_wing;
-			control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
+	case NAVIGATION_STATE_ALTCTL:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
+	case NAVIGATION_STATE_OFFBOARD:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_offboard_enabled = true;
+
+		switch (sp_offboard.mode) {
+		case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = false;
 			control_mode.flag_control_altitude_enabled = false;
 			control_mode.flag_control_climb_rate_enabled = false;
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 			break;
-
-		case MAIN_STATE_ALTCTL:
-			control_mode.flag_control_manual_enabled = true;
-			control_mode.flag_control_auto_enabled = false;
+		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
 			control_mode.flag_control_rates_enabled = true;
 			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = true;
-			control_mode.flag_control_climb_rate_enabled = true;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 			break;
-
-		case MAIN_STATE_POSCTL:
-			control_mode.flag_control_manual_enabled = true;
-			control_mode.flag_control_auto_enabled = false;
+		case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */
+			control_mode.flag_control_climb_rate_enabled = true;
+			control_mode.flag_control_position_enabled = true; /* XXX: hack for now */
+			control_mode.flag_control_velocity_enabled = true;
+			break;
+		case OFFBOARD_CONTROL_MODE_DIRECT_POSITION:
 			control_mode.flag_control_rates_enabled = true;
 			control_mode.flag_control_attitude_enabled = true;
 			control_mode.flag_control_altitude_enabled = true;
@@ -1736,37 +1940,57 @@ set_control_mode()
 			control_mode.flag_control_position_enabled = true;
 			control_mode.flag_control_velocity_enabled = true;
 			break;
-
-		case MAIN_STATE_AUTO:
-			navigator_enabled = true;
-			break;
-
-		case MAIN_STATE_ACRO:
-			control_mode.flag_control_manual_enabled = true;
-			control_mode.flag_control_auto_enabled = false;
-			control_mode.flag_control_rates_enabled = true;
+		default:
+			control_mode.flag_control_rates_enabled = false;
 			control_mode.flag_control_attitude_enabled = false;
 			control_mode.flag_control_altitude_enabled = false;
 			control_mode.flag_control_climb_rate_enabled = false;
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		default:
-			break;
 		}
+		break;
 
+	case NAVIGATION_STATE_POSCTL:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = true;
+		control_mode.flag_control_velocity_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case FAILSAFE_STATE_RTL:
-		navigator_enabled = true;
+	case NAVIGATION_STATE_AUTO_MISSION:
+	case NAVIGATION_STATE_AUTO_LOITER:
+	case NAVIGATION_STATE_AUTO_RTL:
+	case NAVIGATION_STATE_AUTO_RTGS:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = true;
+		control_mode.flag_control_velocity_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case FAILSAFE_STATE_LAND:
-		navigator_enabled = true;
+	case NAVIGATION_STATE_LAND:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		/* in failsafe LAND mode position may be not available */
+		control_mode.flag_control_position_enabled = status.condition_local_position_valid;
+		control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case FAILSAFE_STATE_TERMINATION:
+	case NAVIGATION_STATE_TERMINATION:
 		/* disable all controllers on termination */
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = false;
@@ -1782,33 +2006,16 @@ set_control_mode()
 	default:
 		break;
 	}
-
-	/* navigator has control, set control mode flags according to nav state*/
-	if (navigator_enabled) {
-		control_mode.flag_control_manual_enabled = false;
-		control_mode.flag_control_auto_enabled = true;
-		control_mode.flag_control_rates_enabled = true;
-		control_mode.flag_control_attitude_enabled = true;
-
-		/* in failsafe LAND mode position may be not available */
-		control_mode.flag_control_position_enabled = status.condition_local_position_valid;
-		control_mode.flag_control_velocity_enabled = status.condition_local_position_valid;
-
-		control_mode.flag_control_altitude_enabled = true;
-		control_mode.flag_control_climb_rate_enabled = true;
-	}
 }
 
 void
-print_reject_mode(struct vehicle_status_s *status, const char *msg)
+print_reject_mode(struct vehicle_status_s *status_local, const char *msg)
 {
 	hrt_abstime t = hrt_absolute_time();
 
 	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
 		last_print_mode_reject_time = t;
-		char s[80];
-		sprintf(s, "#audio: REJECT %s", msg);
-		mavlink_log_critical(mavlink_fd, s);
+		mavlink_log_critical(mavlink_fd, "REJECT %s", msg);
 
 		/* only buzz if armed, because else we're driving people nuts indoors
 		they really need to look at the leds as well. */
@@ -1823,9 +2030,7 @@ print_reject_arm(const char *msg)
 
 	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
 		last_print_mode_reject_time = t;
-		char s[80];
-		sprintf(s, "#audio: %s", msg);
-		mavlink_log_critical(mavlink_fd, s);
+		mavlink_log_critical(mavlink_fd, msg);
 		tune_negative(true);
 	}
 }
@@ -1838,12 +2043,12 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 		break;
 
 	case VEHICLE_CMD_RESULT_DENIED:
-		mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
+		mavlink_log_critical(mavlink_fd, "command denied: %u", cmd.command);
 		tune_negative(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_FAILED:
-		mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command);
+		mavlink_log_critical(mavlink_fd, "command failed: %u", cmd.command);
 		tune_negative(true);
 		break;
 
@@ -1854,7 +2059,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 		break;
 
 	case VEHICLE_CMD_RESULT_UNSUPPORTED:
-		mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
+		mavlink_log_critical(mavlink_fd, "command unsupported: %u", cmd.command);
 		tune_negative(true);
 		break;
 
@@ -1939,9 +2144,7 @@ void *commander_low_prio_loop(void *arg)
 				int calib_ret = ERROR;
 
 				/* try to go to INIT/PREFLIGHT arming state */
-
-				// XXX disable interrupts in arming_state_transition
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
 				}
@@ -2004,7 +2207,7 @@ void *commander_low_prio_loop(void *arg)
 					tune_negative(true);
 				}
 
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
 
 				break;
 			}
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 940a04aa1..d5fe122cb 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,11 @@
 /**
  * @file commander_helper.cpp
  * Commander helper functions implementations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ *
  */
 
 #include <stdio.h>
@@ -209,12 +211,18 @@ int led_init()
 	/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
 	(void)ioctl(leds, LED_ON, LED_BLUE);
 
+	/* switch blue off */
+	led_off(LED_BLUE);
+
 	/* we consider the amber led mandatory */
 	if (ioctl(leds, LED_ON, LED_AMBER)) {
 		warnx("Amber LED: ioctl fail\n");
 		return ERROR;
 	}
 
+	/* switch amber off */
+	led_off(LED_AMBER);
+
 	/* then try RGB LEDs, this can fail on FMUv1*/
 	rgbleds = open(RGBLED_DEVICE_PATH, 0);
 
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index e75f2592f..a49c9e263 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,6 +34,9 @@
 /**
  * @file commander_helper.h
  * Commander helper functions definitions
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #ifndef COMMANDER_HELPER_H_
@@ -77,6 +78,8 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern);
  * Use integral of current if battery capacity known (BAT_CAPACITY parameter set),
  * else use simple estimate based on voltage.
  *
+ * @param voltage the current battery voltage
+ * @param discharged the discharged capacity
  * @return the estimated remaining capacity in 0..1
  */
 float battery_remaining_estimate_voltage(float voltage, float discharged);
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 80ca68f21..4750f9d5c 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -39,7 +39,7 @@
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <nuttx/config.h>
@@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3);
  * @group Battery Calibration
  */
 PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
+
+/**
+ * Datalink loss mode enabled.
+ *
+ * Set to 1 to enable actions triggered when the datalink is lost.
+ *
+ * @group commander
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index ee0d08391..2e18c4284 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -183,12 +183,12 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         
         // Safety switch arming tests
         
-        { "transition: init to standby, no safety switch",
+        { "transition: standby to armed, no safety switch",
             { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
             ARMING_STATE_ARMED,
             { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
         
-        { "transition: init to standby, safety switch off",
+        { "transition: standby to armed, safety switch off",
             { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
             ARMING_STATE_ARMED,
             { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
@@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         armed.ready_to_arm = test->current_state.ready_to_arm;
         
         // Attempt transition
-        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed);
+        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */);
         
         // Validate result of transition
         ut_assert(test->assertMsg, test->expected_transition_result == result);
@@ -300,70 +300,151 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
 
 bool StateMachineHelperTest::mainStateTransitionTest(void)
 {
-	struct vehicle_status_s current_state;
-	main_state_t new_main_state;
+	// This structure represent a single test case for testing Main State transitions.
+	typedef struct {
+		const char*     assertMsg;				// Text to show when test case fails
+		uint8_t		condition_bits;				// Bits for various condition_* values
+		main_state_t	from_state;				// State prior to transition request
+		main_state_t	to_state;				// State to transition to
+		transition_result_t	expected_transition_result;	// Expected result from main_state_transition call
+	} MainTransitionTest_t;
+	
+	// Bits for condition_bits
+	#define MTT_ALL_NOT_VALID		0
+	#define MTT_ROTARY_WING			1 << 0
+	#define MTT_LOC_ALT_VALID		1 << 1
+	#define MTT_LOC_POS_VALID		1 << 2
+	#define MTT_HOME_POS_VALID		1 << 3
+	#define MTT_GLOBAL_POS_VALID		1 << 4
+	
+	static const MainTransitionTest_t rgMainTransitionTests[] = {
+		
+		// TRANSITION_NOT_CHANGED tests
+		
+		{ "no transition: identical states",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
+				
+		// TRANSITION_CHANGED tests
+		
+		{ "transition: MANUAL to ACRO",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED },
+
+		{ "transition: ACRO to MANUAL",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to AUTO_MISSION - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
+
+		{ "transition: AUTO_MISSION to MANUAL - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to AUTO_LOITER - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
+
+		{ "transition: AUTO_LOITER to MANUAL - global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
+
+		{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
+			MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to ALTCTL - not rotary",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
+			MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+		
+		{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
+			MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
+		
+		{ "transition: ALTCTL to MANUAL - local altitude valid",
+			MTT_LOC_ALT_VALID,
+			MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to POSCTL - local position not valid, global position valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+
+		{ "transition: MANUAL to POSCTL - local position valid, global position not valid",
+			MTT_LOC_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED },
+
+		{ "transition: POSCTL to MANUAL - local position valid, global position valid",
+			MTT_LOC_POS_VALID,
+			MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
+
+		// TRANSITION_DENIED tests
+
+		{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_LOITER - global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
+			MTT_HOME_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+
+		{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
+			MTT_GLOBAL_POS_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
+			MTT_ROTARY_WING,
+			MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED },
+		
+		{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid",
+			MTT_ALL_NOT_VALID,
+			MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED },
+	};
 	
-	// Identical states.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	new_main_state = MAIN_STATE_MANUAL;
-	ut_assert("no transition: identical states",
-		  TRANSITION_NOT_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);	
-
-	// AUTO to MANUAL.
-	current_state.main_state = MAIN_STATE_AUTO;
-	new_main_state = MAIN_STATE_MANUAL;
-	ut_assert("transition changed: auto to manual",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
-
-	// MANUAL to ALTCTRL.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_altitude_valid = true;
-	new_main_state = MAIN_STATE_ALTCTL;
-	ut_assert("tranisition: manual to altctrl",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state);
-
-	// MANUAL to ALTCTRL, invalid local altitude.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_altitude_valid = false;
-	new_main_state = MAIN_STATE_ALTCTL;
-	ut_assert("no transition: invalid local altitude",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
-
-	// MANUAL to POSCTRL.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_position_valid = true;
-	new_main_state = MAIN_STATE_POSCTL;
-	ut_assert("transition: manual to posctrl",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state);
-
-	// MANUAL to POSCTRL, invalid local position.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_local_position_valid = false;
-	new_main_state = MAIN_STATE_POSCTL;
-	ut_assert("no transition: invalid position",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
-
-	// MANUAL to AUTO.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_global_position_valid = true;
-	new_main_state = MAIN_STATE_AUTO;
-	ut_assert("transition: manual to auto",
-		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
-
-	// MANUAL to AUTO, invalid global position.
-	current_state.main_state = MAIN_STATE_MANUAL;
-	current_state.condition_global_position_valid = false;
-	new_main_state = MAIN_STATE_AUTO;
-	ut_assert("no transition: invalid global position",
-		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+	size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
+	for (size_t i=0; i<cMainTransitionTests; i++) {
+		const MainTransitionTest_t* test = &rgMainTransitionTests[i];
+
+		// Setup initial machine state
+		struct vehicle_status_s current_state;
+		current_state.main_state = test->from_state;
+		current_state.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING;
+		current_state.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
+		current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
+		current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
+		current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
+		
+		// Attempt transition
+		transition_result_t result = main_state_transition(&current_state, test->to_state);
+		
+		// Validate result of transition
+		ut_assert(test->assertMsg, test->expected_transition_result == result);
+		if (test->expected_transition_result == result) {
+			if (test->expected_transition_result == TRANSITION_CHANGED) {
+				ut_assert(test->assertMsg, test->to_state == current_state.main_state);
+			} else {
+				ut_assert(test->assertMsg, test->from_state == current_state.main_state);
+			}
+		}
+	}
+
 
 	return true;
 }
diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h
index a101cd7b1..c14f948cc 100644
--- a/src/modules/commander/mag_calibration.h
+++ b/src/modules/commander/mag_calibration.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index e0f8dc95d..eaf309288 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -16,6 +16,7 @@ enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_POSCTL,
 	PX4_CUSTOM_MAIN_MODE_AUTO,
 	PX4_CUSTOM_MAIN_MODE_ACRO,
+	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {
@@ -25,6 +26,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
 	PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
 	PX4_CUSTOM_SUB_MODE_AUTO_RTL,
 	PX4_CUSTOM_SUB_MODE_AUTO_LAND,
+	PX4_CUSTOM_SUB_MODE_AUTO_RTGS
 };
 
 union px4_custom_mode {
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 87309b238..372ba9d7d 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,6 +34,9 @@
 /**
  * @file state_machine_helper.cpp
  * State machine helper functions implementations
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <stdio.h>
@@ -45,14 +46,19 @@
 #include <dirent.h>
 #include <fcntl.h>
 #include <string.h>
+#include <math.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/airspeed.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_airspeed.h>
 #include <drivers/drv_device.h>
 #include <mavlink/mavlink_log.h>
 
@@ -65,28 +71,24 @@
 #endif
 static const int ERROR = -1;
 
-static bool arming_state_changed = true;
-static bool main_state_changed = true;
-static bool failsafe_state_changed = true;
-
 // This array defines the arming state transitions. The rows are the new state, and the columns
 // are the current state. Using new state and current  state you can index into the array which
 // will be true for a valid transition or false for a invalid transition. In some cases even
 // though the transition is marked as true additional checks must be made. See arming_state_transition
 // code for those checks.
 static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
-	//                                  INIT,   STANDBY,    ARMED,  ARMED_ERROR,    STANDBY_ERROR,  REBOOT,     IN_AIR_RESTORE
-	{ /* ARMING_STATE_INIT */           true,   true,       false,  false,          false,          false,      false },
-	{ /* ARMING_STATE_STANDBY */        true,   true,       true,   true,           false,          false,      false },
-	{ /* ARMING_STATE_ARMED */          false,  true,       true,   false,          false,          false,      true },
-	{ /* ARMING_STATE_ARMED_ERROR */    false,  false,      true,   true,           false,          false,      false },
-	{ /* ARMING_STATE_STANDBY_ERROR */  true,   true,       false,  true,           true,           false,      false },
-	{ /* ARMING_STATE_REBOOT */         true,   true,       false,  false,          true,           true,       true },
-	{ /* ARMING_STATE_IN_AIR_RESTORE */ false,  false,      false,  false,          false,          false,      false }, // NYI
+	//                                  INIT,  STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
+	{ /* ARMING_STATE_INIT */           true,  true,    false, false,       false,         false,  false },
+	{ /* ARMING_STATE_STANDBY */        true,  true,    true,  true,        false,         false,  false },
+	{ /* ARMING_STATE_ARMED */          false, true,    true,  false,       false,         false,  true },
+	{ /* ARMING_STATE_ARMED_ERROR */    false, false,   true,  true,        false,         false,  false },
+	{ /* ARMING_STATE_STANDBY_ERROR */  true,  true,    false, true,        true,          false,  false },
+	{ /* ARMING_STATE_REBOOT */         true,  true,    false, false,       true,          true,   true },
+	{ /* ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,       false,         false,  false }, // NYI
 };
 
 // You can index into the array with an arming_state_t in order to get it's textual representation
-static const char *state_names[ARMING_STATE_MAX] = {
+static const char * const state_names[ARMING_STATE_MAX] = {
 	"ARMING_STATE_INIT",
 	"ARMING_STATE_STANDBY",
 	"ARMING_STATE_ARMED",
@@ -107,18 +109,31 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	ASSERT(ARMING_STATE_INIT == 0);
 	ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
 
-	/*
-	 * Perform an atomic state update
-	 */
-	irqstate_t flags = irqsave();
-
 	transition_result_t ret = TRANSITION_DENIED;
 
+	arming_state_t current_arming_state = status->arming_state;
+
 	/* only check transition if the new state is actually different from the current one */
-	if (new_arming_state == status->arming_state) {
+	if (new_arming_state == current_arming_state) {
 		ret = TRANSITION_NOT_CHANGED;
 
 	} else {
+
+		/*
+		 * Get sensing state if necessary
+		 */
+		int prearm_ret = OK;
+
+		/* only perform the check if we have to */
+		if (new_arming_state == ARMING_STATE_ARMED) {
+			prearm_ret = prearm_check(status, mavlink_fd);
+		}
+
+		/*
+		 * Perform an atomic state update
+		 */
+		irqstate_t flags = irqsave();
+
 		/* enforce lockdown in HIL */
 		if (status->hil_state == HIL_STATE_ON) {
 			armed->lockdown = true;
@@ -133,15 +148,44 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 		if (valid_transition) {
 			// We have a good transition. Now perform any secondary validation.
 			if (new_arming_state == ARMING_STATE_ARMED) {
-				// Fail transition if we need safety switch press
-				//      Allow if coming from in air restore
+
+				//      Do not perform pre-arm checks if coming from in air restore
 				//      Allow if HIL_STATE_ON
-				if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
-					if (mavlink_fd) {
-						mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first.");
+				if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE &&
+					status->hil_state == HIL_STATE_OFF) {
+
+					// Fail transition if pre-arm check fails
+					if (prearm_ret) {
+						valid_transition = false;
+
+					// Fail transition if we need safety switch press
+					} else if (safety->safety_switch_available && !safety->safety_off) {
+
+						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!");
+
+						valid_transition = false;
+					}
+
+					// Perform power checks only if circuit breaker is not
+					// engaged for these checks
+					if (!status->circuit_breaker_engaged_power_check) {
+						// Fail transition if power is not good
+						if (!status->condition_power_input_valid) {
+
+							mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
+							valid_transition = false;
+						}
+
+						// Fail transition if power levels on the avionics rail
+						// are measured but are insufficient
+						if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) &&
+							(status->avionics_power_rail_voltage < 4.9f))) {
+
+							mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
+							valid_transition = false;
+						}
 					}
 
-					valid_transition = false;
 				}
 
 			} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
@@ -165,19 +209,16 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 			armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
 			ret = TRANSITION_CHANGED;
 			status->arming_state = new_arming_state;
-			arming_state_changed = true;
 		}
-	}
 
-	/* end of atomic state update */
-	irqrestore(flags);
+		/* end of atomic state update */
+		irqrestore(flags);
+	}
 
 	if (ret == TRANSITION_DENIED) {
-		static const char *errMsg = "Invalid arming transition from %s to %s";
+		static const char *errMsg = "INVAL: %s - %s";
 
-		if (mavlink_fd) {
-			mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
-		}
+		mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
 
 		warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
 	}
@@ -199,69 +240,67 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet
 	}
 }
 
-bool
-check_arming_state_changed()
-{
-	if (arming_state_changed) {
-		arming_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
 transition_result_t
 main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
-	/* transition may be denied even if requested the same state because conditions may be changed */
+	/* transition may be denied even if the same state is requested because conditions may have changed */
 	switch (new_main_state) {
 	case MAIN_STATE_MANUAL:
-		ret = TRANSITION_CHANGED;
-		break;
-
 	case MAIN_STATE_ACRO:
 		ret = TRANSITION_CHANGED;
 		break;
 
 	case MAIN_STATE_ALTCTL:
-
 		/* need at minimum altitude estimate */
+		/* TODO: add this for fixedwing as well */
 		if (!status->is_rotary_wing ||
 		    (status->condition_local_altitude_valid ||
 		     status->condition_global_position_valid)) {
 			ret = TRANSITION_CHANGED;
 		}
-
 		break;
 
 	case MAIN_STATE_POSCTL:
-
 		/* need at minimum local position estimate */
 		if (status->condition_local_position_valid ||
 		    status->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
-
 		break;
 
-	case MAIN_STATE_AUTO:
-
+	case MAIN_STATE_AUTO_LOITER:
 		/* need global position estimate */
 		if (status->condition_global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
+		break;
+
+	case MAIN_STATE_AUTO_MISSION:
+	case MAIN_STATE_AUTO_RTL:
+		/* need global position and home position */
+		if (status->condition_global_position_valid && status->condition_home_position_valid) {
+			ret = TRANSITION_CHANGED;
+		}
+		break;
+
+	case MAIN_STATE_OFFBOARD:
+
+		/* need offboard signal */
+		if (!status->offboard_control_signal_lost) {
+			ret = TRANSITION_CHANGED;
+		}
 
 		break;
-	}
 
+	case MAIN_STATE_MAX:
+	default:
+		break;
+	}
 	if (ret == TRANSITION_CHANGED) {
 		if (status->main_state != new_main_state) {
 			status->main_state = new_main_state;
-			main_state_changed = true;
-
 		} else {
 			ret = TRANSITION_NOT_CHANGED;
 		}
@@ -270,70 +309,35 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	return ret;
 }
 
-bool
-check_main_state_changed()
-{
-	if (main_state_changed) {
-		main_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
-bool
-check_failsafe_state_changed()
-{
-	if (failsafe_state_changed) {
-		failsafe_state_changed = false;
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
 /**
-* Transition from one hil state to another
-*/
-int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+ * Transition from one hil state to another
+ */
+transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
-	bool valid_transition = false;
-	int ret = ERROR;
+	transition_result_t ret = TRANSITION_DENIED;
 
 	if (current_status->hil_state == new_state) {
-		valid_transition = true;
+		ret = TRANSITION_NOT_CHANGED;
 
 	} else {
-
 		switch (new_state) {
-
 		case HIL_STATE_OFF:
-
 			/* we're in HIL and unexpected things can happen if we disable HIL now */
 			mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
-			valid_transition = false;
-
+			ret = TRANSITION_DENIED;
 			break;
 
 		case HIL_STATE_ON:
-
 			if (current_status->arming_state == ARMING_STATE_INIT
 			    || current_status->arming_state == ARMING_STATE_STANDBY
 			    || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
 
-				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
-				valid_transition = true;
-
-				// Disable publication of all attached sensors
-
+				/* Disable publication of all attached sensors */
 				/* list directory */
 				DIR		*d;
 				d = opendir("/dev");
 
 				if (d) {
-
 					struct dirent	*direntry;
 					char devname[24];
 
@@ -388,290 +392,296 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 
 						printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
 					}
-
 					closedir(d);
+					ret = TRANSITION_CHANGED;
+					mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+
 
 				} else {
 					/* failed opening dir */
-					warnx("FAILED LISTING DEVICE ROOT DIRECTORY");
-					return 1;
+					mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
+					ret = TRANSITION_DENIED;
 				}
+			} else {
+				mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
+				ret = TRANSITION_DENIED;
 			}
-
 			break;
 
 		default:
-			warnx("Unknown hil state");
+			warnx("Unknown HIL state");
 			break;
 		}
 	}
 
-	if (valid_transition) {
+	if (ret == TRANSITION_CHANGED) {
 		current_status->hil_state = new_state;
-
 		current_status->timestamp = hrt_absolute_time();
-		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
 		// XXX also set lockdown here
-
-		ret = OK;
-
-	} else {
-		mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
+		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
 	}
-
 	return ret;
 }
 
-
 /**
-* Transition from one failsafe state to another
-*/
-transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state)
+ * Check failsafe and main status and set navigation status for navigator accordingly
+ */
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
 {
-	transition_result_t ret = TRANSITION_DENIED;
+	navigation_state_t nav_state_old = status->nav_state;
 
-	/* transition may be denied even if requested the same state because conditions may be changed */
-	if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) {
-		/* transitions from TERMINATION to other states not allowed */
-		if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) {
-			ret = TRANSITION_NOT_CHANGED;
+	bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR);
+	status->failsafe = false;
+
+	/* evaluate main state to decide in normal (non-failsafe) mode */
+	switch (status->main_state) {
+	case MAIN_STATE_ACRO:
+	case MAIN_STATE_MANUAL:
+	case MAIN_STATE_ALTCTL:
+	case MAIN_STATE_POSCTL:
+		/* require RC for all manual modes */
+		if (status->rc_signal_lost && armed) {
+			status->failsafe = true;
+
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+
+		} else {
+			switch (status->main_state) {
+			case MAIN_STATE_ACRO:
+				status->nav_state = NAVIGATION_STATE_ACRO;
+				break;
+
+			case MAIN_STATE_MANUAL:
+				status->nav_state = NAVIGATION_STATE_MANUAL;
+				break;
+
+			case MAIN_STATE_ALTCTL:
+				status->nav_state = NAVIGATION_STATE_ALTCTL;
+				break;
+
+			case MAIN_STATE_POSCTL:
+				status->nav_state = NAVIGATION_STATE_POSCTL;
+				break;
+
+			default:
+				status->nav_state = NAVIGATION_STATE_MANUAL;
+				break;
+			}
 		}
+		break;
 
-	} else {
-		switch (new_failsafe_state) {
-		case FAILSAFE_STATE_NORMAL:
-			/* always allowed (except from TERMINATION state) */
-			ret = TRANSITION_CHANGED;
-			break;
+	case MAIN_STATE_AUTO_MISSION:
+		/* go into failsafe
+		 * - if either the datalink is enabled and lost as well as RC is lost
+		 * - if there is no datalink and the mission is finished */
+		if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
+		    (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
+			status->failsafe = true;
 
-		case FAILSAFE_STATE_RTL:
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+
+		/* also go into failsafe if just datalink is lost */
+		} else if (status->data_link_lost && data_link_loss_enabled) {
+			status->failsafe = true;
 
-			/* global position and home position required for RTL */
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->set_nav_state = NAV_STATE_RTL;
-				status->set_nav_state_timestamp = hrt_absolute_time();
-				ret = TRANSITION_CHANGED;
+				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-			break;
+		/* don't bother if RC is lost and mission is not yet finished */
+		} else if (status->rc_signal_lost) {
+
+			/* this mode is ok, we don't need RC for missions */
+			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+		} else {
+			/* everything is perfect */
+			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+		}
+		break;
 
-		case FAILSAFE_STATE_LAND:
+	case MAIN_STATE_AUTO_LOITER:
+		/* go into failsafe if datalink and RC is lost */
+		if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
+			status->failsafe = true;
 
-			/* at least relative altitude estimate required for landing */
-			if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
-				status->set_nav_state = NAV_STATE_LAND;
-				status->set_nav_state_timestamp = hrt_absolute_time();
-				ret = TRANSITION_CHANGED;
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-			break;
+		/* also go into failsafe if just datalink is lost */
+		} else if (status->data_link_lost && data_link_loss_enabled) {
+			status->failsafe = true;
 
-		case FAILSAFE_STATE_TERMINATION:
-			/* always allowed */
-			ret = TRANSITION_CHANGED;
-			break;
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
 
-		default:
-			break;
+		/* go into failsafe if RC is lost and datalink loss is not set up */
+		} else if (status->rc_signal_lost && !data_link_loss_enabled) {
+			status->failsafe = true;
+
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+
+		/* don't bother if RC is lost if datalink is connected */
+		} else if (status->rc_signal_lost) {
+
+			/* this mode is ok, we don't need RC for loitering */
+			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
+		} else {
+			/* everything is perfect */
+			status->nav_state = NAVIGATION_STATE_AUTO_LOITER;
 		}
+		break;
 
-		if (ret == TRANSITION_CHANGED) {
-			if (status->failsafe_state != new_failsafe_state) {
-				status->failsafe_state = new_failsafe_state;
-				failsafe_state_changed = true;
+	case MAIN_STATE_AUTO_RTL:
+		/* require global position and home */
+		if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
+			status->failsafe = true;
 
+			if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
 			} else {
-				ret = TRANSITION_NOT_CHANGED;
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
+		} else {
+			status->nav_state = NAVIGATION_STATE_AUTO_RTL;
+		}
+		break;
+
+	case MAIN_STATE_OFFBOARD:
+		/* require offboard control, otherwise stay where you are */
+		if (status->offboard_control_signal_lost && !status->rc_signal_lost) {
+			status->failsafe = true;
+
+			status->nav_state = NAVIGATION_STATE_POSCTL;
+		} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
+			status->failsafe = true;
+
+			if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+		} else {
+			status->nav_state = NAVIGATION_STATE_OFFBOARD;
 		}
+	default:
+		break;
 	}
 
-	return ret;
+	return status->nav_state != nav_state_old;
 }
 
+int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
+{
+	int ret;
+	bool failed = false;
+
+	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
 
+	if (fd < 0) {
+		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING");
+		failed = true;
+		goto system_eval;
+	}
 
-// /*
-//  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
-//  */
-
-// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
-//  *
-//  * START SUBSYSTEM/EMERGENCY FUNCTIONS
-//  * */
-
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was removed something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was disabled something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-
-///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
-//
-//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
-//{
-//	int ret = 1;
-//
-////	/* Switch on HIL if in standby and not already in HIL mode */
-////	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
-////	    && !current_status->flag_hil_enabled) {
-////		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
-////			/* Enable HIL on request */
-////			current_status->flag_hil_enabled = true;
-////			ret = OK;
-////			state_machine_publish(status_pub, current_status, mavlink_fd);
-////			publish_armed_status(current_status);
-////			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
-////
-////		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
-////			   current_status->flag_fmu_armed) {
-////
-////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
-////
-////		} else {
-////
-////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
-////		}
-////	}
-//
-//	/* switch manual / auto */
-//	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
-//		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
-//
-//	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
-//		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
-//
-//	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
-//		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
-//
-//	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-//		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
-//	}
-//
-//	/* vehicle is disarmed, mode requests arming */
-//	if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-//		/* only arm in standby state */
-//		// XXX REMOVE
-//		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-//			ret = OK;
-//			printf("[cmd] arming due to command request\n");
-//		}
-//	}
-//
-//	/* vehicle is armed, mode requests disarming */
-//	if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-//		/* only disarm in ground ready */
-//		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-//			ret = OK;
-//			printf("[cmd] disarming due to command request\n");
-//		}
-//	}
-//
-//	/* NEVER actually switch off HIL without reboot */
-//	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
-//		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
-//		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
-//		ret = ERROR;
-//	}
-//
-//	return ret;
-//}
+	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
 
+	if (ret != OK) {
+		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION");
+		failed = true;
+		goto system_eval;
+	}
+
+	/* check measurement result range */
+	struct accel_report acc;
+	ret = read(fd, &acc, sizeof(acc));
+
+	if (ret == sizeof(acc)) {
+		/* evaluate values */
+		float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
+
+		if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
+			mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
+			mavlink_log_critical(mavlink_fd, "hold still while arming");
+			/* this is frickin' fatal */
+			failed = true;
+			goto system_eval;
+		}
+	} else {
+		mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ");
+		/* this is frickin' fatal */
+		failed = true;
+		goto system_eval;
+	}
+
+	if (!status->is_rotary_wing) {
+		/* accel done, close it */
+		close(fd);
+		fd = orb_subscribe(ORB_ID(airspeed));
+
+		struct airspeed_s airspeed;
+
+		if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
+			(hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) {
+			mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING");
+			failed = true;
+			goto system_eval;
+		}
+
+		if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
+			mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING");
+			// XXX do not make this fatal yet
+		}
+	}
+
+system_eval:
+	close(fd);
+	return (failed);
+}
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 0ddd4f05a..bb1b87e71 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,14 +34,14 @@
 /**
  * @file state_machine_helper.h
  * State machine helper functions definitions
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #ifndef STATE_MACHINE_HELPER_H_
 #define STATE_MACHINE_HELPER_H_
 
-#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor)
-#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock
-
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
@@ -56,25 +54,17 @@ typedef enum {
 
 } transition_result_t;
 
-transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0);
-
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
-bool check_arming_state_changed();
+transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
+		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
 
 transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
 
-bool check_main_state_changed();
-
-transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state);
-
-bool check_navigation_state_changed();
-
-bool check_failsafe_state_changed();
+transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
 
-void set_navigation_state_changed();
+bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
 
-int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
+int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
 
 #endif /* STATE_MACHINE_HELPER_H_ */