Merge branch 'multirotor' of github.com:cvg/Firmware_Private into fixedwing_l1

ROMFS/px4fmu_common/init.d/rcS

@@ -108,25 +108,17 @@ then
 	nshterm /dev/ttyACM0 &
 	
 	#
-	# Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
+	# Upgrade PX4IO firmware
 	#
-	if [ -f /fs/microsd/px4io.bin ]
+	if px4io update
 	then
-		echo "PX4IO Firmware found. Checking Upgrade.."
+		if [ -d /fs/microsd ]
-		if cmp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
 		then
-			echo "No newer version, skipping upgrade."
+			echo "Flashed PX4IO Firmware OK" > /fs/microsd/px4io.log
-		else
-			echo "Loading /fs/microsd/px4io.bin"
-			if px4io update /fs/microsd/px4io.bin > /fs/microsd/px4io.log
-			then
-				cp /fs/microsd/px4io.bin /fs/microsd/px4io.cur
-				echo "Flashed /fs/microsd/px4io.bin OK" >> /fs/microsd/px4io.log
-			else
-				echo "Failed flashing /fs/microsd/px4io.bin" >> /fs/microsd/px4io.log
-				echo "Failed to upgrade px4io firmware - check if px4io is in bootloader mode"
-			fi
 		fi
+
+		# Allow IO to safely kick back to app
+		usleep 200000
 	fi
 	
 	#

nuttx-configs/px4fmu-v1/nsh/defconfig

@@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=24
+CONFIG_NFILE_DESCRIPTORS=32
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -451,7 +451,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=24
+CONFIG_NFILE_DESCRIPTORS=32
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4

src/drivers/px4io/px4io.cpp

@@ -2096,10 +2096,10 @@ px4io_main(int argc, char *argv[])
 			fn[1] = nullptr;
 
 		} else {
-			fn[0] = "/fs/microsd/px4io.bin";
+			fn[0] = "/etc/extras/px4io-v2_default.bin";
-			fn[1] =	"/etc/px4io.bin";
+			fn[1] = "/etc/extras/px4io-v1_default.bin";
-			fn[2] = "/fs/microsd/px4io2.bin";
+			fn[2] =	"/fs/microsd/px4io.bin";
-			fn[3] =	"/etc/px4io2.bin";
+			fn[3] =	"/fs/microsd/px4io2.bin";
 			fn[4] =	nullptr;
 		}
 

src/drivers/px4io/px4io_uploader.cpp

@@ -82,6 +82,27 @@ PX4IO_Uploader::upload(const char *filenames[])
 #error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload
 #endif
 
+	/* allow an early abort and look for file first */
+	for (unsigned i = 0; filenames[i] != nullptr; i++) {
+		_fw_fd = open(filenames[i], O_RDONLY);
+
+		if (_fw_fd < 0) {
+			log("failed to open %s", filenames[i]);
+			continue;
+		}
+
+		log("using firmware from %s", filenames[i]);
+		filename = filenames[i];
+		break;
+	}
+
+	if (filename == NULL) {
+		log("no firmware found");
+		close(_io_fd);
+		_io_fd = -1;
+		return -ENOENT;
+	}
+
 	_io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR);
 
 	if (_io_fd < 0) {
@@ -106,26 +127,6 @@ PX4IO_Uploader::upload(const char *filenames[])
 		return -EIO;
 	}
 
-	for (unsigned i = 0; filenames[i] != nullptr; i++) {
-		_fw_fd = open(filenames[i], O_RDONLY);
-
-		if (_fw_fd < 0) {
-			log("failed to open %s", filenames[i]);
-			continue;
-		}
-
-		log("using firmware from %s", filenames[i]);
-		filename = filenames[i];
-		break;
-	}
-
-	if (filename == NULL) {
-		log("no firmware found");
-		close(_io_fd);
-		_io_fd = -1;
-		return -ENOENT;
-	}
-
 	struct stat st;
 	if (stat(filename, &st) != 0) {
 		log("Failed to stat %s - %d\n", filename, (int)errno);

src/drivers/px4io/uploader.h

@@ -91,7 +91,7 @@ private:
 	void			drain();
 	int			send(uint8_t c);
 	int			send(uint8_t *p, unsigned count);
-	int			get_sync(unsigned timeout = 1000);
+	int			get_sync(unsigned timeout = 100);
 	int			sync();
 	int			get_info(int param, uint32_t &val);
 	int			erase();

src/modules/commander/commander.cpp

@@ -147,8 +147,6 @@ static bool thread_should_exit = false;		/**< daemon exit flag */
 static bool thread_running = false;		/**< daemon status flag */
 static int daemon_task;				/**< Handle of daemon task / thread */
 
-/* timout until lowlevel failsafe */
-static unsigned int failsafe_lowlevel_timeout_ms;
 static unsigned int leds_counter;
 /* To remember when last notification was sent */
 static uint64_t last_print_mode_reject_time = 0;
@@ -199,6 +197,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
 int commander_thread_main(int argc, char *argv[]);
 
 void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
+
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
 void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
@@ -206,17 +205,20 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl
 transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
 
 void print_reject_mode(const char *msg);
+
 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, struct vehicle_local_position_s *local_pos);
+transition_result_t check_navigation_state_machine(struct vehicle_status_s *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.
  */
 void *commander_low_prio_loop(void *arg);
 
+void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result);
+
 
 int commander_main(int argc, char *argv[])
 {
@@ -271,7 +273,8 @@ void usage(const char *reason)
 	exit(1);
 }
 
-void print_status() {
+void print_status()
+{
 	warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
 
 	/* read all relevant states */
@@ -279,33 +282,40 @@ void print_status() {
 	struct vehicle_status_s state;
 	orb_copy(ORB_ID(vehicle_status), state_sub, &state);
 
-	const char* armed_str;
+	const char *armed_str;
 
 	switch (state.arming_state) {
-		case ARMING_STATE_INIT:
+	case ARMING_STATE_INIT:
-			armed_str = "INIT";
+		armed_str = "INIT";
-			break;
+		break;
-		case ARMING_STATE_STANDBY:
+
-			armed_str = "STANDBY";
+	case ARMING_STATE_STANDBY:
-			break;
+		armed_str = "STANDBY";
-		case ARMING_STATE_ARMED:
+		break;
-			armed_str = "ARMED";
+
-			break;
+	case ARMING_STATE_ARMED:
-		case ARMING_STATE_ARMED_ERROR:
+		armed_str = "ARMED";
-			armed_str = "ARMED_ERROR";
+		break;
-			break;
+
-		case ARMING_STATE_STANDBY_ERROR:
+	case ARMING_STATE_ARMED_ERROR:
-			armed_str = "STANDBY_ERROR";
+		armed_str = "ARMED_ERROR";
-			break;
+		break;
-		case ARMING_STATE_REBOOT:
+
-			armed_str = "REBOOT";
+	case ARMING_STATE_STANDBY_ERROR:
-			break;
+		armed_str = "STANDBY_ERROR";
-		case ARMING_STATE_IN_AIR_RESTORE:
+		break;
-			armed_str = "IN_AIR_RESTORE";
+
-			break;
+	case ARMING_STATE_REBOOT:
-		default:
+		armed_str = "REBOOT";
-			armed_str = "ERR: UNKNOWN STATE";
+		break;
-			break;
+
+	case ARMING_STATE_IN_AIR_RESTORE:
+		armed_str = "IN_AIR_RESTORE";
+		break;
+
+	default:
+		armed_str = "ERR: UNKNOWN STATE";
+		break;
 	}
 
 
@@ -326,7 +336,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			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_main_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;
 
@@ -415,6 +425,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				} else {
 					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 				}
+
 			} else {
 				/* reject TAKEOFF not armed */
 				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
@@ -478,9 +489,6 @@ int commander_thread_main(int argc, char *argv[])
 	bool arm_tune_played = false;
 
 	/* set parameters */
-	failsafe_lowlevel_timeout_ms = 0;
-	param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
-
 	param_t _param_sys_type = param_find("MAV_TYPE");
 	param_t _param_system_id = param_find("MAV_SYS_ID");
 	param_t _param_component_id = param_find("MAV_COMP_ID");
@@ -599,12 +607,6 @@ int commander_thread_main(int argc, char *argv[])
 	unsigned stick_off_counter = 0;
 	unsigned stick_on_counter = 0;
 
-	/* To remember when last notification was sent */
-	uint64_t last_print_control_time = 0;
-
-	enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE;
-	bool armed_previous = false;
-
 	bool low_battery_voltage_actions_done = false;
 	bool critical_battery_voltage_actions_done = false;
 
@@ -665,7 +667,6 @@ int commander_thread_main(int argc, char *argv[])
 	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	struct differential_pressure_s diff_pres;
 	memset(&diff_pres, 0, sizeof(diff_pres));
-	uint64_t last_diff_pres_time = 0;
 
 	/* Subscribe to command topic */
 	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -800,12 +801,15 @@ int commander_thread_main(int argc, char *argv[])
 		/* update condition_local_position_valid and condition_local_altitude_valid */
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(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);
+
 		if (status.condition_local_altitude_valid) {
 			if (status.condition_landed != local_position.landed) {
 				status.condition_landed = local_position.landed;
 				status_changed = true;
+
 				if (status.condition_landed) {
 					mavlink_log_critical(mavlink_fd, "[cmd] LANDED");
+
 				} else {
 					mavlink_log_critical(mavlink_fd, "[cmd] IN AIR");
 				}
@@ -908,6 +912,7 @@ int commander_thread_main(int argc, char *argv[])
 					// XXX should we still allow to arm with critical battery?
 					//arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
 				}
+
 				status_changed = true;
 			}
 
@@ -943,11 +948,6 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
-
-			/* check for first, long-term and valid GPS lock -> set home position */
-			float hdop_m = gps_position.eph_m;
-			float vdop_m = gps_position.epv_m;
-
 			/* check if GPS fix is ok */
 			float hdop_threshold_m = 4.0f;
 			float vdop_threshold_m = 8.0f;
@@ -963,7 +963,7 @@ int commander_thread_main(int argc, char *argv[])
 			 */
 
 			if (!home_position_set && gps_position.fix_type >= 3 &&
-			    (hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) &&	// XXX note that vdop is 0 for mtk
+			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&	// XXX note that vdop is 0 for mtk
 			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
 				/* copy position data to uORB home message, store it locally as well */
 				// TODO use global position estimate
@@ -1013,9 +1013,7 @@ int commander_thread_main(int argc, char *argv[])
 					}
 				}
 
-				status.rc_signal_cutting_off = false;
 				status.rc_signal_lost = false;
-				status.rc_signal_lost_interval = 0;
 
 				transition_result_t res;	// store all transitions results here
 
@@ -1027,10 +1025,10 @@ int commander_thread_main(int argc, char *argv[])
 				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 ||
-				    	(status.condition_landed && (
+				     (status.condition_landed && (
-				   			status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
+					      status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
-			 				status.navigation_state == NAVIGATION_STATE_VECTOR
+					      status.navigation_state == NAVIGATION_STATE_VECTOR
-			    		))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+				      ))) && 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);
@@ -1040,6 +1038,7 @@ int commander_thread_main(int argc, char *argv[])
 					} else {
 						stick_off_counter++;
 					}
+
 				} else {
 					stick_off_counter = 0;
 				}
@@ -1087,7 +1086,7 @@ int commander_thread_main(int argc, char *argv[])
 				res = check_main_state_machine(&status);
 
 				if (res == TRANSITION_CHANGED) {
-					mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
+					//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
 					tune_positive();
 
 				} else if (res == TRANSITION_DENIED) {
@@ -1097,122 +1096,14 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 			} else {
-
+				if (!status.rc_signal_lost) {
-				/* print error message for first RC glitch and then every 5s */
+					mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST");
-				if (!status.rc_signal_cutting_off || (hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL) {
-					// TODO remove debug code
-					if (!status.rc_signal_cutting_off) {
-						warnx("Reason: not rc_signal_cutting_off\n");
-
-					} else {
-						warnx("last print time: %llu\n", last_print_control_time);
-					}
-
-					/* only complain if the offboard control is NOT active */
-					status.rc_signal_cutting_off = true;
-					mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL");
-
-					last_print_control_time = hrt_absolute_time();
-				}
-
-				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-				status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp;
-
-				/* if the RC signal is gone for a full second, consider it lost */
-				if (status.rc_signal_lost_interval > 1000000) {
 					status.rc_signal_lost = true;
-					status.failsave_lowlevel = true;
 					status_changed = true;
 				}
 			}
 		}
 
-		/* END mode switch */
-		/* END RC state check */
-
-		// TODO check this
-		/* state machine update for offboard control */
-		if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) {
-			if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) {	// TODO 5s is too long ?
-
-				// /* decide about attitude control flag, enable in att/pos/vel */
-				// bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
-				// 				  sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
-				// 				  sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
-
-				// /* decide about rate control flag, enable it always XXX (for now) */
-				// bool rates_ctrl_enabled = true;
-
-				// /* set up control mode */
-				// if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
-				// 	current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
-				// 	state_changed = true;
-				// }
-
-				// if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
-				// 	current_status.flag_control_rates_enabled = rates_ctrl_enabled;
-				// 	state_changed = true;
-				// }
-
-				// /* handle the case where offboard control signal was regained */
-				// if (!current_status.offboard_control_signal_found_once) {
-				// 	current_status.offboard_control_signal_found_once = true;
-				// 	/* enable offboard control, disable manual input */
-				// 	current_status.flag_control_manual_enabled = false;
-				// 	current_status.flag_control_offboard_enabled = true;
-				// 	state_changed = true;
-				// 	tune_positive();
-
-				// 	mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST");
-
-				// } else {
-				// 	if (current_status.offboard_control_signal_lost) {
-				// 		mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL");
-				// 		state_changed = true;
-				// 		tune_positive();
-				// 	}
-				// }
-
-				status.offboard_control_signal_weak = false;
-				status.offboard_control_signal_lost = false;
-				status.offboard_control_signal_lost_interval = 0;
-
-				// XXX check if this is correct
-				/* arm / disarm on request */
-				if (sp_offboard.armed && !armed.armed) {
-					arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
-
-				} else if (!sp_offboard.armed && armed.armed) {
-					arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
-				}
-
-			} else {
-
-				/* print error message for first offboard signal glitch and then every 5s */
-				if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL)) {
-					status.offboard_control_signal_weak = true;
-					mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL");
-					last_print_control_time = hrt_absolute_time();
-				}
-
-				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-				status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp;
-
-				/* if the signal is gone for 0.1 seconds, consider it lost */
-				if (status.offboard_control_signal_lost_interval > 100000) {
-					status.offboard_control_signal_lost = true;
-					status.failsave_lowlevel_start_time = hrt_absolute_time();
-					tune_positive();
-
-					/* kill motors after timeout */
-					if (hrt_absolute_time() > status.failsave_lowlevel_start_time + failsafe_lowlevel_timeout_ms * 1000) {
-						status.failsave_lowlevel = true;
-						status_changed = true;
-					}
-				}
-			}
-		}
-
 		/* evaluate the navigation state machine */
 		transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
 
@@ -1230,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[])
 		if (arming_state_changed || main_state_changed || navigation_state_changed) {
 			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
 			status_changed = true;
+
 		} else {
 			status_changed = false;
 		}
@@ -1288,10 +1180,6 @@ int commander_thread_main(int argc, char *argv[])
 			arm_tune_played = false;
 		}
 
-		/* store old modes to detect and act on state transitions */
-		battery_warning_previous = status.battery_warning;
-		armed_previous = armed.armed;
-
 		fflush(stdout);
 		counter++;
 
@@ -1343,6 +1231,7 @@ void
 toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
 {
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
 	/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
 	if (armed->armed) {
 		/* armed, solid */
@@ -1352,11 +1241,13 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 		/* ready to arm, blink at 1Hz */
 		if (leds_counter % 20 == 0)
 			led_toggle(LED_BLUE);
+
 	} else {
 		/* not ready to arm, blink at 10Hz */
 		if (leds_counter % 2 == 0)
 			led_toggle(LED_BLUE);
 	}
+
 #endif
 
 	if (changed) {
@@ -1369,50 +1260,60 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 			/* armed, solid */
 			if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
 				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
+
 			} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
 				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
+
 			} else {
-				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN :RGBLED_COLOR_GREEN;
+				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
 			}
+
 			pattern.duration[0] = 1000;
 
 		} else if (armed->ready_to_arm) {
-			for (i=0; i<3; i++) {
+			for (i = 0; i < 3; i++) {
 				if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
-					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
+					pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
-				} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
-					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
-				} else {
-					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
-				}
-				pattern.duration[i*2] = 200;
 
-				pattern.color[i*2+1] = RGBLED_COLOR_OFF;
+				} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
-				pattern.duration[i*2+1] = 800;
+					pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
-			}
+
-			if (status->condition_global_position_valid) {
+				} else {
-				pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
+					pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
-				pattern.duration[i*2] = 1000;
-				pattern.color[i*2+1] = RGBLED_COLOR_OFF;
-				pattern.duration[i*2+1] = 800;
-			} else {
-				for (i=3; i<6; i++) {
-					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
-					pattern.duration[i*2] = 100;
-					pattern.color[i*2+1] = RGBLED_COLOR_OFF;
-					pattern.duration[i*2+1] = 100;
 				}
-				pattern.color[6*2] = RGBLED_COLOR_OFF;
+
-				pattern.duration[6*2] = 700;
+				pattern.duration[i * 2] = 200;
+
+				pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
+				pattern.duration[i * 2 + 1] = 800;
+			}
+
+			if (status->condition_global_position_valid) {
+				pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
+				pattern.duration[i * 2] = 1000;
+				pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
+				pattern.duration[i * 2 + 1] = 800;
+
+			} else {
+				for (i = 3; i < 6; i++) {
+					pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
+					pattern.duration[i * 2] = 100;
+					pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
+					pattern.duration[i * 2 + 1] = 100;
+				}
+
+				pattern.color[6 * 2] = RGBLED_COLOR_OFF;
+				pattern.duration[6 * 2] = 700;
 			}
 
 		} else {
-			for (i=0; i<3; i++) {
+			for (i = 0; i < 3; i++) {
-				pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
+				pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
-				pattern.duration[i*2] = 200;
+				pattern.duration[i * 2] = 200;
-				pattern.color[i*2+1] = RGBLED_COLOR_OFF;
+				pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
-				pattern.duration[i*2+1] = 200;
+				pattern.duration[i * 2 + 1] = 200;
 			}
+
 			/* not ready to arm, blink at 10Hz */
 		}
 
@@ -1423,6 +1324,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 	if (status->load > 0.95f) {
 		if (leds_counter % 2 == 0)
 			led_toggle(LED_AMBER);
+
 	} else {
 		led_off(LED_AMBER);
 	}
@@ -1572,70 +1474,124 @@ 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, struct vehicle_local_position_s *local_pos)
+check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
 {
 	transition_result_t res = TRANSITION_DENIED;
 
-	switch (current_status->main_state) {
+	if (status->main_state == MAIN_STATE_AUTO) {
-	case MAIN_STATE_MANUAL:
+		if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
-		res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
+			if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-		break;
-
-	case MAIN_STATE_SEATBELT:
-		res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode);
-		break;
-
-	case MAIN_STATE_EASY:
-		res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode);
-		break;
-
-	case MAIN_STATE_AUTO:
-		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) {
+				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);
-				// TRANSITION_DENIED is not possible here
-				break;
 
-			} else {
+			if (res != TRANSITION_NOT_CHANGED) {
-				/* if not landed: */
+				/* check again, state can be changed */
-				if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
+				if (status->condition_landed) {
-					/* act depending on switches when manual control enabled */
+					/* if landed: transitions only to AUTO_READY are allowed */
-					if (current_status->return_switch == RETURN_SWITCH_RETURN) {
+					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
-						/* RTL */
+					// TRANSITION_DENIED is not possible here
-						res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
-
-					} else {
-						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 {
-					/* switch to mission in air when no RC control */
+					/* not landed */
-					res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
+					if (status->rc_signal_found_once && !status->rc_signal_lost) {
+						/* act depending on switches when manual control enabled */
+						if (status->return_switch == RETURN_SWITCH_RETURN) {
+							/* RTL */
+							res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
+
+						} else {
+							if (status->mission_switch == MISSION_SWITCH_MISSION) {
+								/* MISSION */
+								res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
+
+							} else {
+								/* LOITER */
+								res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
+							}
+						}
+
+					} else {
+						/* switch to MISSION in air when no RC control */
+						if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
+						    status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
+						    status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
+						    status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+							res = TRANSITION_NOT_CHANGED;
+
+						} else {
+							res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
+						}
+					}
 				}
 			}
-		} else {
-			res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
-		}
-		break;
 
-	default:
+		} else {
-		break;
+			/* disarmed, always switch to AUTO_READY */
+			res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
+		}
+
+	} else {
+		/* manual control modes */
+		if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) {
+			/* switch to failsafe mode */
+			bool manual_control_old = control_mode->flag_control_manual_enabled;
+
+			if (!status->condition_landed) {
+				/* in air: try to hold position */
+				res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
+
+			} else {
+				/* landed: don't try to hold position but land (if taking off) */
+				res = TRANSITION_DENIED;
+			}
+
+			if (res == TRANSITION_DENIED) {
+				res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
+			}
+
+			control_mode->flag_control_manual_enabled = false;
+
+			if (res == TRANSITION_NOT_CHANGED && manual_control_old) {
+				/* mark navigation state as changed to force immediate flag publishing */
+				set_navigation_state_changed();
+				res = TRANSITION_CHANGED;
+			}
+
+			if (res == TRANSITION_CHANGED) {
+				if (control_mode->flag_control_position_enabled) {
+					mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD");
+
+				} else {
+					if (status->condition_landed) {
+						mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)");
+
+					} else {
+						mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD");
+					}
+				}
+			}
+
+		} else {
+			switch (status->main_state) {
+			case MAIN_STATE_MANUAL:
+				res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
+				break;
+
+			case MAIN_STATE_SEATBELT:
+				res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
+				break;
+
+			case MAIN_STATE_EASY:
+				res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
+				break;
+
+			default:
+				break;
+			}
+		}
 	}
 
 	return res;
@@ -1644,27 +1600,32 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
 void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
 {
 	switch (result) {
-		case VEHICLE_CMD_RESULT_ACCEPTED:
+	case VEHICLE_CMD_RESULT_ACCEPTED:
 			tune_positive();
-			break;
+		break;
-		case VEHICLE_CMD_RESULT_DENIED:
+
-			mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
+	case VEHICLE_CMD_RESULT_DENIED:
-			tune_negative();
+		mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
-			break;
+		tune_negative();
-		case VEHICLE_CMD_RESULT_FAILED:
+		break;
-			mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
+
-			tune_negative();
+	case VEHICLE_CMD_RESULT_FAILED:
-			break;
+		mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
-		case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
+		tune_negative();
-			mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
+		break;
-			tune_negative();
+
-			break;
+	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
-		case VEHICLE_CMD_RESULT_UNSUPPORTED:
+		mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
-			mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
+		tune_negative();
-			tune_negative();
+		break;
-			break;
+
-		default:
+	case VEHICLE_CMD_RESULT_UNSUPPORTED:
-			break;
+		mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
+		tune_negative();
+		break;
+
+	default:
+		break;
 	}
 }
 
@@ -1720,11 +1681,13 @@ void *commander_low_prio_loop(void *arg)
 					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 {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 				}
@@ -1732,83 +1695,85 @@ void *commander_low_prio_loop(void *arg)
 			} else {
 				answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 			}
-		break;
+
+			break;
 
 		case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
 
-			int calib_ret = ERROR;
+				int calib_ret = ERROR;
 
-			/* try to go to INIT/PREFLIGHT arming state */
+				/* 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)) {
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+					break;
+				}
+
+				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);
+				}
+
+				if (calib_ret == OK)
+					tune_positive();
+				else
+					tune_negative();
+
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 
-			// 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;
 			}
 
-			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);
-			}
-
-			if (calib_ret == OK)
-				tune_positive();
-			else
-				tune_negative();
-
-			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
-
-			break;
-		}
-
 		case VEHICLE_CMD_PREFLIGHT_STORAGE: {
 
-			if (((int)(cmd.param1)) == 0) {
+				if (((int)(cmd.param1)) == 0) {
-				if (0 == param_load_default()) {
+					if (0 == param_load_default()) {
-					mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
+						mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
-					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
-				} else {
+					} else {
-					mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+						mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
-					answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
+						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");
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+
+					} else {
+						mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
+					}
 				}
 
-			} else if (((int)(cmd.param1)) == 1) {
+				break;
-				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");
-					answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
-				}
 			}
-			break;
-		}
 
 		default:
 			answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
@@ -1817,7 +1782,7 @@ void *commander_low_prio_loop(void *arg)
 
 		/* send any requested ACKs */
 		if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE
-			&& cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {
+		    && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {
 			/* send acknowledge command */
 			// XXX TODO
 		}

src/modules/commander/commander_params.c

@@ -45,7 +45,7 @@
 #include <nuttx/config.h>
 #include <systemlib/param/param.h>
 
-PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0);	/**< Go into low-level failsafe after 0 ms */
+PARAM_DEFINE_INT32(SYS_FAILSAFE_LL, 0);	/**< Go into low-level failsafe after 0 ms */
 PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);

src/modules/commander/state_machine_helper.cpp

@@ -184,7 +184,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 	return ret;
 }
 
-bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed)
+bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed)
 {
 	// System is safe if:
 	// 1) Not armed
@@ -276,12 +276,12 @@ check_main_state_changed()
 }
 
 transition_result_t
-navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
+navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
 {
 	transition_result_t ret = TRANSITION_DENIED;
 
 	/* only check transition if the new state is actually different from the current one */
-	if (new_navigation_state == current_status->navigation_state) {
+	if (new_navigation_state == status->navigation_state) {
 		ret = TRANSITION_NOT_CHANGED;
 
 	} else {
@@ -296,6 +296,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 			control_mode->flag_control_altitude_enabled = false;
 			control_mode->flag_control_climb_rate_enabled = false;
 			control_mode->flag_control_manual_enabled = true;
+			control_mode->flag_control_auto_enabled = false;
 			break;
 
 		case NAVIGATION_STATE_STABILIZE:
@@ -307,6 +308,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 			control_mode->flag_control_altitude_enabled = false;
 			control_mode->flag_control_climb_rate_enabled = false;
 			control_mode->flag_control_manual_enabled = true;
+			control_mode->flag_control_auto_enabled = false;
 			break;
 
 		case NAVIGATION_STATE_ALTHOLD:
@@ -318,6 +320,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 			control_mode->flag_control_altitude_enabled = true;
 			control_mode->flag_control_climb_rate_enabled = true;
 			control_mode->flag_control_manual_enabled = true;
+			control_mode->flag_control_auto_enabled = false;
 			break;
 
 		case NAVIGATION_STATE_VECTOR:
@@ -329,6 +332,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 			control_mode->flag_control_altitude_enabled = true;
 			control_mode->flag_control_climb_rate_enabled = true;
 			control_mode->flag_control_manual_enabled = true;
+			control_mode->flag_control_auto_enabled = false;
 			break;
 
 		case NAVIGATION_STATE_AUTO_READY:
@@ -340,12 +344,13 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 			control_mode->flag_control_altitude_enabled = false;
 			control_mode->flag_control_climb_rate_enabled = false;
 			control_mode->flag_control_manual_enabled = false;
+			control_mode->flag_control_auto_enabled = true;
 			break;
 
 		case NAVIGATION_STATE_AUTO_TAKEOFF:
 
 			/* only transitions from AUTO_READY */
-			if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
+			if (status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
 				ret = TRANSITION_CHANGED;
 				control_mode->flag_control_rates_enabled = true;
 				control_mode->flag_control_attitude_enabled = true;
@@ -354,6 +359,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 				control_mode->flag_control_altitude_enabled = true;
 				control_mode->flag_control_climb_rate_enabled = true;
 				control_mode->flag_control_manual_enabled = false;
+				control_mode->flag_control_auto_enabled = true;
 			}
 
 			break;
@@ -367,6 +373,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 			control_mode->flag_control_altitude_enabled = true;
 			control_mode->flag_control_climb_rate_enabled = true;
 			control_mode->flag_control_manual_enabled = false;
+			control_mode->flag_control_auto_enabled = false;
 			break;
 
 		case NAVIGATION_STATE_AUTO_MISSION:
@@ -378,6 +385,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 			control_mode->flag_control_altitude_enabled = true;
 			control_mode->flag_control_climb_rate_enabled = true;
 			control_mode->flag_control_manual_enabled = false;
+			control_mode->flag_control_auto_enabled = true;
 			break;
 
 		case NAVIGATION_STATE_AUTO_RTL:
@@ -389,12 +397,13 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 			control_mode->flag_control_altitude_enabled = true;
 			control_mode->flag_control_climb_rate_enabled = true;
 			control_mode->flag_control_manual_enabled = false;
+			control_mode->flag_control_auto_enabled = true;
 			break;
 
 		case NAVIGATION_STATE_AUTO_LAND:
 
 			/* deny transitions from landed state */
-			if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
+			if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
 				ret = TRANSITION_CHANGED;
 				control_mode->flag_control_rates_enabled = true;
 				control_mode->flag_control_attitude_enabled = true;
@@ -403,6 +412,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 				control_mode->flag_control_altitude_enabled = true;
 				control_mode->flag_control_climb_rate_enabled = true;
 				control_mode->flag_control_manual_enabled = false;
+				control_mode->flag_control_auto_enabled = true;
 			}
 
 			break;
@@ -412,8 +422,8 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 		}
 
 		if (ret == TRANSITION_CHANGED) {
-			current_status->navigation_state = new_navigation_state;
+			status->navigation_state = new_navigation_state;
-			control_mode->auto_state = current_status->navigation_state;
+			control_mode->auto_state = status->navigation_state;
 			navigation_state_changed = true;
 		}
 	}
@@ -433,6 +443,12 @@ check_navigation_state_changed()
 	}
 }
 
+void
+set_navigation_state_changed()
+{
+	navigation_state_changed = true;
+}
+
 /**
 * Transition from one hil state to another
 */

src/modules/commander/state_machine_helper.h

@@ -58,7 +58,7 @@ 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);
+		arming_state_t new_arming_state, struct actuator_armed_s *armed);
 
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
@@ -68,10 +68,12 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
 
 bool check_main_state_changed();
 
-transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
+transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
 
 bool check_navigation_state_changed();
 
+void set_navigation_state_changed();
+
 int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
 
 #endif /* STATE_MACHINE_HELPER_H_ */

src/modules/mavlink/mavlink.c

@@ -240,11 +240,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 	 **/
 
 	/* set calibration state */
-	if (v_status.preflight_calibration) {
+	if (v_status.arming_state == ARMING_STATE_INIT
-		*mavlink_state = MAV_STATE_CALIBRATING;
-	} else if (v_status.system_emergency) {
-		*mavlink_state = MAV_STATE_EMERGENCY;
-	} else if (v_status.arming_state == ARMING_STATE_INIT
 			|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
 			|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
 		*mavlink_state = MAV_STATE_UNINIT;

src/modules/multirotor_att_control/multirotor_att_control_main.c

@@ -2,6 +2,7 @@
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,6 +39,7 @@
  * Implementation of multirotor attitude control main loop.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
@@ -63,6 +65,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/parameter_update.h>
@@ -74,8 +77,6 @@
 #include "multirotor_attitude_control.h"
 #include "multirotor_rate_control.h"
 
-PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost.
-
 __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
 
 static bool thread_should_exit;
@@ -102,7 +103,8 @@ mc_thread_main(int argc, char *argv[])
 	memset(&offboard_sp, 0, sizeof(offboard_sp));
 	struct vehicle_rates_setpoint_s rates_sp;
 	memset(&rates_sp, 0, sizeof(rates_sp));
-
+	struct vehicle_status_s status;
+	memset(&status, 0, sizeof(status));
 	struct actuator_controls_s actuators;
 	memset(&actuators, 0, sizeof(actuators));
 
@@ -114,6 +116,8 @@ mc_thread_main(int argc, char *argv[])
 	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+	int status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	/*
 	 * Do not rate-limit the loop to prevent aliasing
@@ -136,7 +140,6 @@ mc_thread_main(int argc, char *argv[])
 	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 	orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
-	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 
 	/* register the perf counter */
 	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
@@ -149,12 +152,6 @@ mc_thread_main(int argc, char *argv[])
 	/* store last control mode to detect mode switches */
 	bool control_yaw_position = true;
 	bool reset_yaw_sp = true;
-	bool failsafe_first_time = true;
-
-	/* prepare the handle for the failsafe throttle */
-	param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
-	float failsafe_throttle = 0.0f;
-	param_get(failsafe_throttle_handle, &failsafe_throttle);
 
 	while (!thread_should_exit) {
 
@@ -176,7 +173,6 @@ mc_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(parameter_update), param_sub, &update);
 
 				/* update parameters */
-				param_get(failsafe_throttle_handle, &failsafe_throttle);
 			}
 
 			/* only run controller if attitude changed */
@@ -205,6 +201,13 @@ mc_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
 				}
 
+				/* get a local copy of status */
+				orb_check(status_sub, &updated);
+
+				if (updated) {
+					orb_copy(ORB_ID(vehicle_status), status_sub, &status);
+				}
+
 				/* get a local copy of the current sensor values */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
 
@@ -244,47 +247,18 @@ mc_thread_main(int argc, char *argv[])
 					/* manual input */
 					if (control_mode.flag_control_attitude_enabled) {
 						/* control attitude, update attitude setpoint depending on mode */
-						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
+						if (att_sp.thrust < 0.1f) {
-						if (control_mode.failsave_highlevel) {
+							/* no thrust, don't try to control yaw */
-							failsafe_first_time = false;
+							rates_sp.yaw = 0.0f;
+							control_yaw_position = false;
 
-							if (!control_mode.flag_control_velocity_enabled) {
+							if (status.condition_landed) {
-								/* don't reset attitude setpoint in position control mode, it's handled by position controller. */
+								/* reset yaw setpoint if on ground */
-								att_sp.roll_body = 0.0f;
-								att_sp.pitch_body = 0.0f;
-							}
-
-							if (!control_mode.flag_control_climb_rate_enabled) {
-								/* don't touch throttle in modes with altitude hold, it's handled by position controller.
-								 *
-								 * Only go to failsafe throttle if last known throttle was
-								 * high enough to create some lift to make hovering state likely.
-								 *
-								 * This is to prevent that someone landing, but not disarming his
-								 * multicopter (throttle = 0) does not make it jump up in the air
-								 * if shutting down his remote.
-								 */
-								if (isfinite(manual.throttle) && manual.throttle > min_takeoff_throttle) {	// TODO use landed status instead of throttle
-									/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
-									att_sp.thrust = failsafe_throttle;
-
-								} else {
-									att_sp.thrust = 0.0f;
-								}
-							}
-
-							/* keep current yaw, do not attempt to go to north orientation,
-							 * since if the pilot regains RC control, he will be lost regarding
-							 * the current orientation.
-							 */
-							if (failsafe_first_time) {
 								reset_yaw_sp = true;
 							}
 
 						} else {
-							failsafe_first_time = true;
+							/* only move yaw setpoint if manual input is != 0 */
-
-							/* only move yaw setpoint if manual input is != 0 and not landed */
 							if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
 								/* control yaw rate */
 								control_yaw_position = false;
@@ -294,18 +268,17 @@ mc_thread_main(int argc, char *argv[])
 							} else {
 								control_yaw_position = true;
 							}
+						}
 
-							if (!control_mode.flag_control_velocity_enabled) {
+						if (!control_mode.flag_control_velocity_enabled) {
-								/* update attitude setpoint if not in position control mode */
+							/* update attitude setpoint if not in position control mode */
-								att_sp.roll_body = manual.roll;
+							att_sp.roll_body = manual.roll;
-								att_sp.pitch_body = manual.pitch;
+							att_sp.pitch_body = manual.pitch;
 
-								if (!control_mode.flag_control_climb_rate_enabled) {
+							if (!control_mode.flag_control_climb_rate_enabled) {
-									/* pass throttle directly if not in altitude control mode */
+								/* pass throttle directly if not in altitude control mode */
-									att_sp.thrust = manual.throttle;
+								att_sp.thrust = manual.throttle;
-								}
 							}
-
 						}
 
 						/* reset yaw setpint to current position if needed */
@@ -324,7 +297,7 @@ mc_thread_main(int argc, char *argv[])
 
 						att_sp.timestamp = hrt_absolute_time();
 
-						/* publish the controller output */
+						/* publish the attitude setpoint */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 
 					} else {
@@ -342,6 +315,17 @@ mc_thread_main(int argc, char *argv[])
 					}
 
 				} else {
+					if (!control_mode.flag_control_auto_enabled) {
+						/* no control, try to stay on place */
+						if (!control_mode.flag_control_velocity_enabled) {
+							/* no velocity control, reset attitude setpoint */
+							att_sp.roll_body = 0.0f;
+							att_sp.pitch_body = 0.0f;
+							att_sp.timestamp = hrt_absolute_time();
+							orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+						}
+					}
+
 					/* reset yaw setpoint after non-manual control */
 					reset_yaw_sp = true;
 				}

src/modules/multirotor_pos_control/multirotor_pos_control.c

@@ -415,10 +415,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 				/* force reprojection of global setpoint after manual mode */
 				global_pos_sp_reproject = true;
 
-			} else {
+			} else if (control_mode.flag_control_auto_enabled) {
-				/* non-manual mode, use global setpoint */
+				/* AUTO mode, use global setpoint */
 				if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
 					reset_auto_pos = true;
+
 				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
 					if (reset_auto_pos) {
 						local_pos_sp.x = local_pos.x;
@@ -428,21 +429,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 						local_pos_sp_valid = true;
 						att_sp.yaw_body = att.yaw;
 						reset_auto_pos = false;
-						mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, local_pos_sp.z);
+						mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, -local_pos_sp.z);
-					}
-				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) {
-					if (reset_auto_pos) {
-						local_pos_sp.x = local_pos.x;
-						local_pos_sp.y = local_pos.y;
-						local_pos_sp.z = local_pos.z;
-						local_pos_sp.yaw = att.yaw;
-						local_pos_sp_valid = true;
-						att_sp.yaw_body = att.yaw;
-						reset_auto_pos = false;
 					}
+
 				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
 					// TODO
 					reset_auto_pos = true;
+
 				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
 					/* init local projection using local position ref */
 					if (local_pos.ref_timestamp != local_ref_timestamp) {
@@ -457,6 +450,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					if (global_pos_sp_reproject) {
 						/* update global setpoint projection */
 						global_pos_sp_reproject = false;
+
 						if (global_pos_sp_valid) {
 							/* global position setpoint valid, use it */
 							double sp_lat = global_pos_sp.lat * 1e-7;
@@ -470,6 +464,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 							} else {
 								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
 							}
+
 							local_pos_sp.yaw = global_pos_sp.yaw;
 							att_sp.yaw_body = global_pos_sp.yaw;
 
@@ -489,6 +484,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 							mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
 						}
 					}
+
 					reset_auto_pos = true;
 				}
 
@@ -496,9 +492,44 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					global_pos_sp_reproject = true;
 				}
 
-				/* reset setpoints after non-manual modes */
+				/* reset setpoints after AUTO mode */
 				reset_sp_xy = true;
 				reset_sp_z = true;
+
+			} else {
+				/* no control, loiter or stay on ground */
+				if (local_pos.landed) {
+					/* landed: move setpoint down */
+					/* in air: hold altitude */
+					if (local_pos_sp.z < 5.0f) {
+						/* set altitude setpoint to 5m under ground,
+						 * don't set it too deep to avoid unexpected landing in case of false "landed" signal */
+						local_pos_sp.z = 5.0f;
+						mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", -local_pos_sp.z);
+					}
+
+					reset_sp_z = true;
+
+				} else {
+					/* in air: hold altitude */
+					if (reset_sp_z) {
+						reset_sp_z = false;
+						local_pos_sp.z = local_pos.z;
+						mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", -local_pos_sp.z);
+					}
+				}
+
+				if (control_mode.flag_control_position_enabled) {
+					if (reset_sp_xy) {
+						reset_sp_xy = false;
+						local_pos_sp.x = local_pos.x;
+						local_pos_sp.y = local_pos.y;
+						local_pos_sp.yaw = att.yaw;
+						local_pos_sp_valid = true;
+						att_sp.yaw_body = att.yaw;
+						mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", local_pos_sp.x, local_pos_sp.y);
+					}
+				}
 			}
 
 			/* publish local position setpoint */

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -429,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (fds[6].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
 
-				if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 5.0f) {
+				if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 4.0f) {
 					/* initialize reference position if needed */
 					if (!ref_xy_inited) {
 						if (ref_xy_init_start == 0) {

src/modules/uORB/topics/vehicle_control_mode.h

@@ -80,9 +80,7 @@ struct vehicle_control_mode_s
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
 	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */
 
-	bool failsave_lowlevel;				/**< Set to true if low-level failsafe mode is enabled */
+	bool flag_control_auto_enabled;		// TEMP
-	bool failsave_highlevel;			/**< Set to true if high-level failsafe mode is enabled */
-
 	uint8_t auto_state;	// TEMP navigation state for AUTO modes
 };
 

src/modules/uORB/topics/vehicle_status.h

@@ -174,8 +174,6 @@ struct vehicle_status_s
 
 	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
-	uint64_t failsave_lowlevel_start_time;		/**< time when the lowlevel failsafe flag was set */
-//	uint64_t failsave_highlevel_begin; TO BE COMPLETED
 
 	main_state_t main_state;				/**< main state machine */
 	navigation_state_t navigation_state;	/**< navigation state machine */
@@ -207,22 +205,13 @@ struct vehicle_status_s
 	bool condition_landed;					/**< true if vehicle is landed, always true if disarmed */
 
 	bool rc_signal_found_once;
-	bool rc_signal_lost;				/**< true if RC reception is terminally lost */
+	bool rc_signal_lost;				/**< true if RC reception lost */
-	bool rc_signal_cutting_off;			/**< true if RC reception is weak / cutting off */
-	uint64_t rc_signal_lost_interval;		/**< interval in microseconds since when no RC signal is available */
 
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
 	bool offboard_control_signal_weak;
 	uint64_t offboard_control_signal_lost_interval;	/**< interval in microseconds without an offboard control message */
 
-	bool failsave_lowlevel;				/**< Set to true if low-level failsafe mode is enabled */
-	bool failsave_highlevel;
-
-	bool preflight_calibration;
-
-	bool system_emergency;
-
 	/* see SYS_STATUS mavlink message for the following */
 	uint32_t onboard_control_sensors_present;
 	uint32_t onboard_control_sensors_enabled;