State machine / switching improvements

4 files changed, 31 insertions, 78 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index f47e48191..3ad90c029 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1778,20 +1778,16 @@ int commander_thread_main(int argc, char *argv[])
 				} else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) {
 					/* check auto mode switch for correct mode */
 					if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
-						/* enable stabilized mode */
-						update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
+						/* enable guided mode */
+						update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
 
 					} else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
 						update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
 
-					} else {
-						update_state_machine_mode_hold(stat_pub, &current_status, mavlink_fd);
 					}
 				} else {
 					/* center stick position, set SAS for all vehicle types */
-					current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-					current_status.flag_control_attitude_enabled = true;
-					current_status.flag_control_rates_enabled = true;
+					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
 				}
 
 				/* handle the case where RC signal was regained */
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index fdd32ca40..18337a4ee 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -548,40 +548,41 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
 
 void update_state_machine_mode_stabilized(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. STAB MODE");
-		tune_error();
-		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");
+	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
+		printf("[cmd] att 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;
+		int old_manual_control_mode = current_status->manual_control_mode;
+		current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
+		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
 		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);
+		current_status->flag_control_manual_enabled = true;
+		if (old_mode != current_status->flight_mode ||
+			old_manual_control_mode != current_status->manual_control_mode) {
+			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
+			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)
+void update_state_machine_mode_guided(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");
+		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
+		tune_error();
 		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");
+		printf("[cmd] position guided mode\n");
 		int old_mode = current_status->flight_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_HOLD;
+		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);
+
 	}
 }
 
@@ -634,6 +635,8 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
 	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
 		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
+	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
+		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
 	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
 		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
 	}
diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h
index 815645089..2f2ccc729 100644
--- a/apps/commander/state_machine_helper.h
+++ b/apps/commander/state_machine_helper.h
@@ -128,13 +128,13 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
 void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
 /**
- * Handle state machine if mode switch is hold
+ * Handle state machine if mode switch is guided
  *
  * @param status_pub file descriptor for state update topic publication
  * @param current_status pointer to the current state machine to operate on
  * @param mavlink_fd file descriptor for MAVLink statustext messages
  */
-void update_state_machine_mode_hold(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
 /**
  * Handle state machine if mode switch is auto
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c
index 646deb62e..a36f1ce7d 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c
@@ -185,55 +185,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 
 			/* control */
 
-			if (vstatus.state_machine == SYSTEM_STATE_AUTO) {
-				/* attitude control */
-				fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-
-				/* angular rate control */
-				fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-
-				/* pass through throttle */
-				actuators.control[3] = att_sp.thrust;
-
-				/* set flaps to zero */
-				actuators.control[4] = 0.0f;
-
-			} else if (vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
-
-				/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-				if (vstatus.rc_signal_lost) {
-					
-					// XXX define failsafe throttle param
-					//param_get(failsafe_throttle_handle, &failsafe_throttle);
-					att_sp.roll_body = 0.3f;
-					att_sp.pitch_body = 0.0f;
-					att_sp.yaw_body = 0;
-					
-					/* limit throttle to 60 % of last value */
-					if (isfinite(manual_sp.throttle)) {
-						att_sp.thrust = 0.6f * manual_sp.throttle;
-					} else {
-						att_sp.thrust = 0.0f;
-					}
-
-					// XXX disable yaw control, loiter
-
-				} else {
-					
-					att_sp.roll_body = manual_sp.roll;
-					att_sp.pitch_body = manual_sp.pitch;
-					att_sp.yaw_body = 0;
-					att_sp.thrust = manual_sp.throttle;
-				}
-
-				att_sp.timestamp = hrt_absolute_time();
-
-				// XXX: Stop copying setpoint / reference from bus, instead keep position
-				// and mix RC inputs in.
-				// XXX: For now just stabilize attitude, not anything else
-				// proper implementation should do stabilization in position controller
-				// and just check for stabilized or auto state
-
+			if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
+				vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
 				/* attitude control */
 				fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
 
@@ -252,13 +205,14 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 					/* if the RC signal is lost, try to stay level and go slowly back down to ground */
 					if (vstatus.rc_signal_lost) {
 						
-						// XXX define failsafe throttle param
-						//param_get(failsafe_throttle_handle, &failsafe_throttle);
+						/* put plane into loiter */
 						att_sp.roll_body = 0.3f;
 						att_sp.pitch_body = 0.0f;
 
-						/* limit throttle to 60 % of last value */
-						if (isfinite(manual_sp.throttle)) {
+						/* limit throttle to 60 % of last value if sane */
+						if (isfinite(manual_sp.throttle) && 
+							(manual_sp.throttle >= 0.0f) &&
+							(manual_sp.throttle <= 1.0f)) {
 							att_sp.thrust = 0.6f * manual_sp.throttle;
 						} else {
 							att_sp.thrust = 0.0f;