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

1 files changed, 54 insertions, 37 deletions

diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c
index 3e1fc4952..9c450e68e 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c
@@ -195,44 +195,61 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 				/* pass through throttle */
 				actuators.control[3] = att_sp.thrust;
 
-			} 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.thrust = 0.5f;
-
-					// 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();
+				/* set flaps to zero */
+				actuators.control[4] = 0.0f;
+
+			} else {
+				if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+
+					/* 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.thrust = 0.5f;
+
+						// 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();
+					}
+
+					/* 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;
+
+					/* pass through flaps */
+					if (isfinite(manual_sp.flaps)) {
+						actuators.control[4] = manual_sp.flaps;
+					} else {
+						actuators.control[4] = 0.0f;
+					}
+
+				} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+					/* directly pass through values */
+					actuators.control[0] = manual_sp.roll;
+					/* positive pitch means negative actuator -> pull up */
+					actuators.control[1] = manual_sp.pitch;
+					actuators.control[2] = manual_sp.yaw;
+					actuators.control[3] = manual_sp.throttle;
+					if (isfinite(manual_sp.flaps)) {
+						actuators.control[4] = manual_sp.flaps;
+					} else {
+						actuators.control[4] = 0.0f;
+					}
 				}
-
-				/* 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;
-
-			} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
-				/* directly pass through values */
-				actuators.control[0] = manual_sp.roll;
-				/* positive pitch means negative actuator -> pull up */
-				actuators.control[1] = manual_sp.pitch;
-				actuators.control[2] = manual_sp.yaw;
-				actuators.control[3] = manual_sp.throttle;
 			}
 
 			/* publish rates */