Cleaned up control mode state machine / flight mode / navigation state machine still needs a bit cleaning up

1 files changed, 90 insertions, 54 deletions

diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index c4b9fbb6a..4152142df 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -93,8 +93,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* set system flags according to state */
 		current_status->flag_system_armed = true;
 
-		fprintf(stderr, "[commander] EMERGENCY LANDING!\n");
-		mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!");
+		fprintf(stderr, "[cmd] EMERGENCY LANDING!\n");
+		mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY LANDING!");
 		break;
 
 	case SYSTEM_STATE_EMCY_CUTOFF:
@@ -103,8 +103,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* set system flags according to state */
 		current_status->flag_system_armed = false;
 
-		fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n");
-		mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!");
+		fprintf(stderr, "[cmd] EMERGENCY MOTOR CUTOFF!\n");
+		mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY MOTOR CUTOFF!");
 		break;
 
 	case SYSTEM_STATE_GROUND_ERROR:
@@ -114,8 +114,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* prevent actuators from arming */
 		current_status->flag_system_armed = false;
 
-		fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n");
-		mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system");
+		fprintf(stderr, "[cmd] GROUND ERROR, locking down propulsion system\n");
+		mavlink_log_critical(mavlink_fd, "[cmd] GROUND ERROR, locking down propulsion system");
 		break;
 
 	case SYSTEM_STATE_PREFLIGHT:
@@ -123,10 +123,10 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		 || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
 			/* set system flags according to state */
 			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state");
+			mavlink_log_critical(mavlink_fd, "[cmd] Switched to PREFLIGHT state");
 		} else {
 			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state");
+			mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to switch to PREFLIGHT state");
 		}
 		break;
 
@@ -136,13 +136,13 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 			invalid_state = false;
 			/* set system flags according to state */
 			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
+			mavlink_log_critical(mavlink_fd, "[cmd] REBOOTING SYSTEM");
 			usleep(500000);
 			up_systemreset();
 			/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
 		} else {
 			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT");
+			mavlink_log_critical(mavlink_fd, "[cmd] REFUSED to REBOOT");
 		}
 		break;
 
@@ -152,7 +152,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* standby enforces disarmed */
 		current_status->flag_system_armed = false;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state");
+		mavlink_log_critical(mavlink_fd, "[cmd] Switched to STANDBY state");
 		break;
 
 	case SYSTEM_STATE_GROUND_READY:
@@ -162,7 +162,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* ground ready has motors / actuators armed */
 		current_status->flag_system_armed = true;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state");
+		mavlink_log_critical(mavlink_fd, "[cmd] Switched to GROUND READY state");
 		break;
 
 	case SYSTEM_STATE_AUTO:
@@ -172,7 +172,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* auto is airborne and in auto mode, motors armed */
 		current_status->flag_system_armed = true;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode");
+		mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / AUTO mode");
 		break;
 
 	case SYSTEM_STATE_STABILIZED:
@@ -180,7 +180,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* set system flags according to state */
 		current_status->flag_system_armed = true;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode");
+		mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / STABILIZED mode");
 		break;
 
 	case SYSTEM_STATE_MANUAL:
@@ -188,7 +188,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* set system flags according to state */
 		current_status->flag_system_armed = true;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode");
+		mavlink_log_critical(mavlink_fd, "[cmd] Switched to FLYING / MANUAL mode");
 		break;
 
 	default:
@@ -203,7 +203,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		ret = OK;
 	}
 	if (invalid_state) {
-		mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition");
+		mavlink_log_critical(mavlink_fd, "[cmd] REJECTING invalid state transition");
 		ret = ERROR;
 	}
 	return ret;
@@ -232,7 +232,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
 
 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-	printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
+	printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
 }
 
 void publish_armed_status(const struct vehicle_status_s *current_status) {
@@ -250,7 +250,7 @@ void publish_armed_status(const struct vehicle_status_s *current_status) {
  */
 void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
-	fprintf(stderr, "[commander] EMERGENCY HANDLER\n");
+	fprintf(stderr, "[cmd] EMERGENCY HANDLER\n");
 	/* Depending on the current state go to one of the error states */
 
 	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
@@ -262,7 +262,7 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat
 		//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
 
 	} else {
-		fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine);
+		fprintf(stderr, "[cmd] Unknown system state: #%d\n", current_status->state_machine);
 	}
 }
 
@@ -272,7 +272,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st
 		state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
 
 	} else {
-		//global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
+		//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
 	}
 
 }
@@ -497,7 +497,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_
 void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
 	if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
-		printf("[commander] arming\n");
+		printf("[cmd] arming\n");
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
 	}
 }
@@ -505,11 +505,11 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s
 void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-		printf("[commander] going standby\n");
+		printf("[cmd] going standby\n");
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
 
 	} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[commander] MISSION ABORT!\n");
+		printf("[cmd] MISSION ABORT!\n");
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
 	}
 }
@@ -518,6 +518,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
 {
 	int old_mode = current_status->flight_mode;
 	current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
+
 	current_status->flag_control_manual_enabled = true;
 	/* enable attitude control per default */
 	current_status->flag_control_attitude_enabled = true;
@@ -525,58 +526,91 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
 	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[commander] manual mode\n");
+		printf("[cmd] manual mode\n");
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
 	}
 }
 
 void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
-	int old_mode = current_status->flight_mode;
-	current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
-	current_status->flag_control_manual_enabled = true;
-	current_status->flag_control_attitude_enabled = true;
-	current_status->flag_control_rates_enabled = true;
-	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
+	if (!current_status->flag_vector_flight_mode_ok) {
+		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. STAB MODE");
+		return;
+	}
+	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
+		printf("[cmd] stabilized mode\n");
+		int old_mode = current_status->flight_mode;
+		current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
+		current_status->flag_control_manual_enabled = false;
+		current_status->flag_control_attitude_enabled = true;
+		current_status->flag_control_rates_enabled = true;
+		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
+		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
+
+	}
+}
 
+void update_state_machine_mode_hold(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	if (!current_status->flag_vector_flight_mode_ok) {
+		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. HOLD MODE");
+		return;
+	}
+	
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[commander] stabilized mode\n");
+		printf("[cmd] stabilized mode\n");
+		int old_mode = current_status->flight_mode;
+		current_status->flight_mode = VEHICLE_FLIGHT_MODE_HOLD;
+		current_status->flag_control_manual_enabled = false;
+		current_status->flag_control_attitude_enabled = true;
+		current_status->flag_control_rates_enabled = true;
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
+		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 	}
 }
 
 void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
-	int old_mode = current_status->flight_mode;
-	current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
-	current_status->flag_control_manual_enabled = true;
-	current_status->flag_control_attitude_enabled = true;
-	current_status->flag_control_rates_enabled = true;
-	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-
+	if (!current_status->flag_vector_flight_mode_ok) {
+		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
+		return;
+	}
+	
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
-		printf("[commander] auto mode\n");
+		printf("[cmd] auto mode\n");
+		int old_mode = current_status->flight_mode;
+		current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
+		current_status->flag_control_manual_enabled = false;
+		current_status->flag_control_attitude_enabled = true;
+		current_status->flag_control_rates_enabled = true;
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
+		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 	}
 }
 
 
 uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
 {
-	printf("[commander] Requested new mode: %d\n", (int)mode);
 	uint8_t ret = 1;
     
-    /* Switch on HIL if in standby and not already in HIL mode */
-	if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
+	/* Switch on HIL if in standby and not already in HIL mode */
+	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
 		&& !current_status->flag_hil_enabled) {
-		/* 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("[commander] 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) {
-		mavlink_log_critical(mavlink_fd, "[commander] REJECTING HIL, not in standby.")
+		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_system_armed) {
+
+			mavlink_log_critical(mavlink_fd, "[cmd] REJECTING HIL, disarm first!")
+		} else {
+
+			mavlink_log_critical(mavlink_fd, "[cmd] REJECTING HIL, not in standby.")
+		}
 	}
     
 	/* switch manual / auto */
@@ -595,7 +629,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 		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("[commander] arming due to command request\n");
+			printf("[cmd] arming due to command request\n");
 		}
 	}
 
@@ -605,13 +639,14 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 		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("[commander] disarming due to command request\n");
+			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)) {
-		fprintf(stderr, "[commander] DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
+		fprintf(stderr, "[cmd] DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
+		mavlink_log_critical(mavlink_fd, "[cmd] Power-cycle to exit HIL");
 		ret = ERROR;
 	}
 
@@ -636,7 +671,8 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
 
 		if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
 			printf("system will reboot\n");
-			//global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO);
+			mavlink_log_critical(mavlink_fd, "[cmd] Rebooting..");
+			usleep(200000);
 			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
 			ret = 0;
 		}