State machine / switching improvements

1 files changed, 7 insertions, 53 deletions

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;