1 files changed, 168 insertions, 142 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 17db0f9c8..d90008633 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -84,6 +84,7 @@
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 #include <systemlib/cpuload.h>
+#include <systemlib/rc_check.h>
 
 #include "px4_custom_mode.h"
 #include "commander_helper.h"
@@ -209,7 +210,7 @@ void print_reject_arm(const char *msg);
 
 void print_status();
 
-transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode);
+transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
 
 /**
  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
@@ -322,10 +323,10 @@ 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;
+			uint8_t custom_main_mode = (uint8_t) cmd->param2;
 
 			// TODO remove debug code
-			mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode);
+			mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
 			/* set arming state */
 			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
@@ -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_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_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_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_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */
 					main_res = main_state_transition(status, MAIN_STATE_AUTO);
 				}
@@ -502,7 +503,13 @@ int commander_thread_main(int argc, char *argv[])
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
 	if (mavlink_fd < 0) {
-		warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.");
+		/* try again later */
+		usleep(20000);
+		mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+		if (mavlink_fd < 0) {
+			warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first.");
+		}
 	}
 
 	/* Main state machine */
@@ -610,6 +617,8 @@ int commander_thread_main(int argc, char *argv[])
 
 	bool updated = false;
 
+	bool rc_calibration_ok = (OK == rc_calibration_check());
+
 	/* Subscribe to safety topic */
 	int safety_sub = orb_subscribe(ORB_ID(safety));
 	memset(&safety, 0, sizeof(safety));
@@ -720,6 +729,9 @@ int commander_thread_main(int argc, char *argv[])
 				param_get(_param_system_id, &(status.system_id));
 				param_get(_param_component_id, &(status.component_id));
 				status_changed = true;
+
+				/* Re-check RC calibration */
+				rc_calibration_ok = (OK == rc_calibration_check());
 			}
 		}
 
@@ -793,9 +805,9 @@ int commander_thread_main(int argc, char *argv[])
 				status.condition_landed = local_position.landed;
 				status_changed = true;
 				if (status.condition_landed) {
-					mavlink_log_info(mavlink_fd, "[cmd] LANDED");
+					mavlink_log_critical(mavlink_fd, "[cmd] LANDED");
 				} else {
-					mavlink_log_info(mavlink_fd, "[cmd] IN AIR");
+					mavlink_log_critical(mavlink_fd, "[cmd] IN AIR");
 				}
 			}
 		}
@@ -1010,46 +1022,42 @@ int commander_thread_main(int argc, char *argv[])
 				/* arm/disarm by RC */
 				res = TRANSITION_NOT_CHANGED;
 
-				/* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm
+				/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
 				 * do it only for rotary wings */
 				if (status.is_rotary_wing &&
 				    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
-				    (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY)) {
-					if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-						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);
-							res = arming_state_transition(&status, &safety, new_arming_state, &armed);
-							stick_off_counter = 0;
-
-						} else {
-							stick_off_counter++;
-						}
-
-						stick_on_counter = 0;
+				    (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY ||
+				    	(status.condition_landed && (
+				   			status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
+			 				status.navigation_state == NAVIGATION_STATE_VECTOR
+			    		))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+					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);
+						res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+						stick_off_counter = 0;
 
 					} else {
-						stick_off_counter = 0;
+						stick_off_counter++;
 					}
+				} else {
+					stick_off_counter = 0;
 				}
 
-				/* check if left stick is in lower right position and we're in manual mode -> arm */
+				/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
 				if (status.arming_state == ARMING_STATE_STANDBY &&
-				    status.main_state == MAIN_STATE_MANUAL) {
-					if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-						if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-							res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
-							stick_on_counter = 0;
-
-						} else {
-							stick_on_counter++;
-						}
-
-						stick_off_counter = 0;
+				    status.main_state == MAIN_STATE_MANUAL &&
+				    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+						res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						stick_on_counter = 0;
 
 					} else {
-						stick_on_counter = 0;
+						stick_on_counter++;
 					}
+
+				} else {
+					stick_on_counter = 0;
 				}
 
 				if (res == TRANSITION_CHANGED) {
@@ -1206,7 +1214,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* evaluate the navigation state machine */
-		transition_result_t res = check_navigation_state_machine(&status, &control_mode);
+		transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
 
 		if (res == TRANSITION_DENIED) {
 			/* DENIED here indicates bug in the commander */
@@ -1564,7 +1572,7 @@ print_reject_arm(const char *msg)
 }
 
 transition_result_t
-check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode)
+check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
 {
 	transition_result_t res = TRANSITION_DENIED;
 
@@ -1582,11 +1590,15 @@ 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->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) {
+			if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+				/* don't switch to other states until takeoff not completed */
+				if (local_pos->z > -5.0f || status.condition_landed) {
+					res = TRANSITION_NOT_CHANGED;
+					break;
+				}
+			}
+			/* check again, state can be changed */
 			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);
@@ -1613,12 +1625,13 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
 					}
 
 				} else {
-					/* always switch to loiter in air when no RC control */
-					res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
+					/* switch to mission in air when no RC control */
+					res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
 				}
 			}
+		} else {
+			res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
 		}
-
 		break;
 
 	default:
@@ -1628,6 +1641,33 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
 	return res;
 }
 
+void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
+{
+	switch (result) {
+		case VEHICLE_CMD_RESULT_ACCEPTED:
+			tune_positive();
+			break;
+		case VEHICLE_CMD_RESULT_DENIED:
+			mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
+			tune_negative();
+			break;
+		case VEHICLE_CMD_RESULT_FAILED:
+			mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
+			tune_negative();
+			break;
+		case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
+			mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
+			tune_negative();
+			break;
+		case VEHICLE_CMD_RESULT_UNSUPPORTED:
+			mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
+			tune_negative();
+			break;
+		default:
+			break;
+	}
+}
+
 void *commander_low_prio_loop(void *arg)
 {
 	/* Set thread name */
@@ -1665,128 +1705,114 @@ void *commander_low_prio_loop(void *arg)
 
 		/* ignore commands the high-prio loop handles */
 		if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
-		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM)
+		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
+		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
 			continue;
 
-		/* result of the command */
-		uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-
 		/* only handle low-priority commands here */
 		switch (cmd.command) {
 
 		case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
-				if (is_safe(&status, &safety, &armed)) {
-
-					if (((int)(cmd.param1)) == 1) {
-						/* reboot */
-						systemreset(false);
-					} else if (((int)(cmd.param1)) == 3) {
-						/* reboot to bootloader */
-						systemreset(true);
-					} else {
-						result = VEHICLE_CMD_RESULT_DENIED;
-					}
-				
+			if (is_safe(&status, &safety, &armed)) {
+
+				if (((int)(cmd.param1)) == 1) {
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					usleep(1000000);
+					/* reboot */
+					systemreset(false);
+				} else if (((int)(cmd.param1)) == 3) {
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					usleep(1000000);
+					/* reboot to bootloader */
+					systemreset(true);
 				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 				}
-			break;
 
-		case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
+			} else {
+				answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+			}
+		break;
 
-				/* try to go to INIT/PREFLIGHT arming state */
+		case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
 
-				// XXX disable interrupts in arming_state_transition
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
-					result = VEHICLE_CMD_RESULT_DENIED;
-					break;
-				}
+			int calib_ret = ERROR;
 
-				if ((int)(cmd.param1) == 1) {
-					/* gyro calibration */
-					do_gyro_calibration(mavlink_fd);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+			/* try to go to INIT/PREFLIGHT arming state */
 
-				} else if ((int)(cmd.param2) == 1) {
-					/* magnetometer calibration */
-					do_mag_calibration(mavlink_fd);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+			// XXX disable interrupts in arming_state_transition
+			if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
+				answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+				break;
+			}
 
-				} else if ((int)(cmd.param3) == 1) {
-					/* zero-altitude pressure calibration */
-					result = VEHICLE_CMD_RESULT_DENIED;
-					
-				} else if ((int)(cmd.param4) == 1) {
-					/* RC calibration */
-					result = VEHICLE_CMD_RESULT_DENIED;
-
-				} else if ((int)(cmd.param5) == 1) {
-					/* accelerometer calibration */
-					do_accel_calibration(mavlink_fd);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
+			if ((int)(cmd.param1) == 1) {
+				/* gyro calibration */
+				answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+				calib_ret = do_gyro_calibration(mavlink_fd);
+
+			} else if ((int)(cmd.param2) == 1) {
+				/* magnetometer calibration */
+				answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+				calib_ret = do_mag_calibration(mavlink_fd);
+
+			} else if ((int)(cmd.param3) == 1) {
+				/* zero-altitude pressure calibration */
+				answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+
+			} else if ((int)(cmd.param4) == 1) {
+				/* RC calibration */
+				answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+
+			} else if ((int)(cmd.param5) == 1) {
+				/* accelerometer calibration */
+				answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+				calib_ret = do_accel_calibration(mavlink_fd);
+
+			} else if ((int)(cmd.param6) == 1) {
+				/* airspeed calibration */
+				answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+				calib_ret = do_airspeed_calibration(mavlink_fd);
+			}
 
-				} else if ((int)(cmd.param6) == 1) {
-					/* airspeed calibration */
-					do_airspeed_calibration(mavlink_fd);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-				}
+			if (calib_ret == OK)
+				tune_positive();
+			else
+				tune_negative();
 
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 
-				break;
-			}
+			break;
+		}
 
 		case VEHICLE_CMD_PREFLIGHT_STORAGE: {
 
-				if (((int)(cmd.param1)) == 0) {
-					if (0 == param_load_default()) {
-						mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
+			if (((int)(cmd.param1)) == 0) {
+				if (0 == param_load_default()) {
+					mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
-					} else {
-						mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
-						tune_error();
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
+				} else {
+					mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+					answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
+				}
 
-				} else if (((int)(cmd.param1)) == 1) {
-					if (0 == param_save_default()) {
-						mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
+			} else if (((int)(cmd.param1)) == 1) {
+				if (0 == param_save_default()) {
+					mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
-					} else {
-						mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
-						tune_error();
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
+				} else {
+					mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+					answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 				}
-
-				break;
 			}
-
-		default:
 			break;
 		}
 
-		/* supported command handling stop */
-		if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
-			tune_positive();
-
-		} else {
-			tune_negative();
-
-			if (result == VEHICLE_CMD_RESULT_DENIED) {
-				mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
-
-			} else if (result == VEHICLE_CMD_RESULT_FAILED) {
-				mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
-
-			} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
-				mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
-
-			} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
-				mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
-			}
+		default:
+			answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
+			break;
 		}
 
 		/* send any requested ACKs */