Merge branch 'seatbelt_multirotor_new' of github.com:PX4/Firmware into multirotor

4 files changed, 84 insertions, 42 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 4f4907dc8..3654839fb 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -322,7 +322,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	switch (cmd->command) {
 	case VEHICLE_CMD_DO_SET_MODE: {
 			uint8_t base_mode = (uint8_t) cmd->param1;
-			uint32_t custom_mode = (uint32_t) cmd->param2;
+			union px4_custom_mode custom_mode;
+			custom_mode.data_float = cmd->param2;
 
 			// TODO remove debug code
 			mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode);
@@ -355,19 +356,19 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
 				/* use autopilot-specific mode */
-				if (custom_mode == PX4_CUSTOM_MODE_MANUAL) {
+				if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
 					/* MANUAL */
 					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
 
-				} else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) {
+				} else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
 					/* SEATBELT */
 					main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
 
-				} else if (custom_mode == PX4_CUSTOM_MODE_EASY) {
+				} else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
 					/* EASY */
 					main_res = main_state_transition(status, MAIN_STATE_EASY);
 
-				} else if (custom_mode == PX4_CUSTOM_MODE_AUTO) {
+				} else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */
 					main_res = main_state_transition(status, MAIN_STATE_AUTO);
 				}
@@ -1588,43 +1589,46 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
 		break;
 
 	case MAIN_STATE_AUTO:
-		if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-			/* don't act while taking off */
-			res = TRANSITION_NOT_CHANGED;
-
-		} else {
-			if (current_status->condition_landed) {
-				/* if landed: transitions only to AUTO_READY are allowed */
-				res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
-				// TRANSITION_DENIED is not possible here
-				break;
+		if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) {
+			if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+				/* don't act while taking off */
+				res = TRANSITION_NOT_CHANGED;
 
 			} else {
-				/* if not landed: */
-				if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
-					/* act depending on switches when manual control enabled */
-					if (current_status->return_switch == RETURN_SWITCH_RETURN) {
-						/* RTL */
-						res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
+				if (current_status->condition_landed) {
+					/* if landed: transitions only to AUTO_READY are allowed */
+					res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
+					// TRANSITION_DENIED is not possible here
+					break;
 
-					} else {
-						if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
-							/* MISSION */
-							res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
+				} else {
+					/* if not landed: */
+					if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
+						/* act depending on switches when manual control enabled */
+						if (current_status->return_switch == RETURN_SWITCH_RETURN) {
+							/* RTL */
+							res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
 
 						} else {
-							/* LOITER */
-							res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
+							if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
+								/* MISSION */
+								res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
+
+							} else {
+								/* LOITER */
+								res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
+							}
 						}
-					}
 
-				} else {
-					/* always switch to loiter in air when no RC control */
-					res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
+					} else {
+						/* always switch to loiter in air when no RC control */
+						res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
+					}
 				}
 			}
+		} else {
+			res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
 		}
-
 		break;
 
 	default:
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index 4d4859a5c..b60a7e0b9 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -8,11 +8,30 @@
 #ifndef PX4_CUSTOM_MODE_H_
 #define PX4_CUSTOM_MODE_H_
 
-enum PX4_CUSTOM_MODE {
-	PX4_CUSTOM_MODE_MANUAL = 1,
-	PX4_CUSTOM_MODE_SEATBELT,
-	PX4_CUSTOM_MODE_EASY,
-	PX4_CUSTOM_MODE_AUTO,
+enum PX4_CUSTOM_MAIN_MODE {
+	PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
+	PX4_CUSTOM_MAIN_MODE_SEATBELT,
+	PX4_CUSTOM_MAIN_MODE_EASY,
+	PX4_CUSTOM_MAIN_MODE_AUTO,
+};
+
+enum PX4_CUSTOM_SUB_MODE_AUTO {
+	PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
+	PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
+	PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
+	PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
+	PX4_CUSTOM_SUB_MODE_AUTO_RTL,
+	PX4_CUSTOM_SUB_MODE_AUTO_LAND,
+};
+
+union px4_custom_mode {
+	struct {
+		uint16_t reserved;
+		uint8_t main_mode;
+		uint8_t sub_mode;
+	};
+	uint32_t data;
+	float data_float;
 };
 
 #endif /* PX4_CUSTOM_MODE_H_ */
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index f39bbaa72..0a506b1a9 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -205,19 +205,35 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 
 	/* main state */
 	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+	union px4_custom_mode custom_mode;
+	custom_mode.data = 0;
 	if (v_status.main_state == MAIN_STATE_MANUAL) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
-		*mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
 	} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
-		*mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
 	} else if (v_status.main_state == MAIN_STATE_EASY) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-		*mavlink_custom_mode = PX4_CUSTOM_MODE_EASY;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
 	} else if (v_status.main_state == MAIN_STATE_AUTO) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-		*mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
+		}
 	}
+	*mavlink_custom_mode = custom_mode.data;
 
 	/**
 	 * Set mavlink state
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 28f7af33c..eb28af1a1 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -71,6 +71,7 @@
 #include <systemlib/err.h>
 #include <systemlib/airspeed.h>
 #include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
 
 __BEGIN_DECLS
 
@@ -196,9 +197,11 @@ handle_message(mavlink_message_t *msg)
 		mavlink_set_mode_t new_mode;
 		mavlink_msg_set_mode_decode(msg, &new_mode);
 
+		union px4_custom_mode custom_mode;
+		custom_mode.data = new_mode.custom_mode;
 		/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
 		vcmd.param1 = new_mode.base_mode;
-		vcmd.param2 = new_mode.custom_mode;
+		vcmd.param2 = custom_mode.data_float;
 		vcmd.param3 = 0;
 		vcmd.param4 = 0;
 		vcmd.param5 = 0;