Stabilization enabling / switching

4 files changed, 107 insertions, 62 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index be50289c3..ab50bab16 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1684,63 +1684,63 @@ int commander_thread_main(int argc, char *argv[])
 
 			if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
 
-				/*
-				 * Check if manual control modes have to be switched
-				 */
-				if (!isfinite(sp_man.manual_mode_switch)) {
-					printf("man mode sw not finite\n");
-
-					/* this switch is not properly mapped, set default */
-					if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
-						(current_status.system_type == MAV_TYPE_HEXAROTOR) ||
-						(current_status.system_type == MAV_TYPE_OCTOROTOR)) {
-
-						/* assuming a rotary wing, fall back to SAS */
-						current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-						current_status.flag_control_attitude_enabled = true;
-						current_status.flag_control_rates_enabled = true;
-					} else {
-
-						/* assuming a fixed wing, fall back to direct pass-through */
-						current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-						current_status.flag_control_attitude_enabled = false;
-						current_status.flag_control_rates_enabled = false;
-					}
-
-				} else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) {
-
-					/* bottom stick position, set direct mode for vehicles supporting it */
-					if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
-						(current_status.system_type == MAV_TYPE_HEXAROTOR) ||
-						(current_status.system_type == MAV_TYPE_OCTOROTOR)) {
-
-						/* assuming a rotary wing, fall back to SAS */
-						current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-						current_status.flag_control_attitude_enabled = true;
-						current_status.flag_control_rates_enabled = true;
-					} else {
-
-						/* assuming a fixed wing, set to direct pass-through as requested */
-						current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-						current_status.flag_control_attitude_enabled = false;
-						current_status.flag_control_rates_enabled = false;
-					}
+				// /*
+				//  * Check if manual control modes have to be switched
+				//  */
+				// if (!isfinite(sp_man.manual_mode_switch)) {
+				// 	printf("man mode sw not finite\n");
+
+				// 	/* this switch is not properly mapped, set default */
+				// 	if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
+				// 		(current_status.system_type == MAV_TYPE_HEXAROTOR) ||
+				// 		(current_status.system_type == MAV_TYPE_OCTOROTOR)) {
+
+				// 		/* assuming a rotary wing, fall back to SAS */
+				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
+				// 		current_status.flag_control_attitude_enabled = true;
+				// 		current_status.flag_control_rates_enabled = true;
+				// 	} else {
+
+				// 		/* assuming a fixed wing, fall back to direct pass-through */
+				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
+				// 		current_status.flag_control_attitude_enabled = false;
+				// 		current_status.flag_control_rates_enabled = false;
+				// 	}
+
+				// } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) {
+
+				// 	/* bottom stick position, set direct mode for vehicles supporting it */
+				// 	if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
+				// 		(current_status.system_type == MAV_TYPE_HEXAROTOR) ||
+				// 		(current_status.system_type == MAV_TYPE_OCTOROTOR)) {
+
+				// 		/* assuming a rotary wing, fall back to SAS */
+				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
+				// 		current_status.flag_control_attitude_enabled = true;
+				// 		current_status.flag_control_rates_enabled = true;
+				// 	} else {
+
+				// 		/* assuming a fixed wing, set to direct pass-through as requested */
+				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
+				// 		current_status.flag_control_attitude_enabled = false;
+				// 		current_status.flag_control_rates_enabled = false;
+				// 	}
+
+				// } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) {
+
+				// 	/* top 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;
 
-				} else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) {
-
-					/* top 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;
-
-				} else {
-					/* center stick position, set rate control */
-					current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
-					current_status.flag_control_attitude_enabled = false;
-					current_status.flag_control_rates_enabled = true;
-				}
+				// } else {
+				// 	/* center stick position, set rate control */
+				// 	current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
+				// 	current_status.flag_control_attitude_enabled = false;
+				// 	current_status.flag_control_rates_enabled = true;
+				// }
 
-				printf("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
+				// printf("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
 
 				/*
 				 * Check if manual stability control modes have to be switched
@@ -1801,7 +1801,7 @@ int commander_thread_main(int argc, char *argv[])
 				if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) {
 					/* enable manual override */
 					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
-				} else {
+				} 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 */
@@ -1813,6 +1813,11 @@ int commander_thread_main(int argc, char *argv[])
 					} 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;
 				}
 
 				/* 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 a8cef8c01..f30fd975b 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -520,9 +520,24 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
 	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;
-	current_status->flag_control_rates_enabled = true;
+
+	/* set behaviour based on airframe */
+	if ((current_status.system_type == MAV_TYPE_QUADROTOR) ||
+		(current_status.system_type == MAV_TYPE_HEXAROTOR) ||
+		(current_status.system_type == MAV_TYPE_OCTOROTOR)) {
+
+		/* assuming a rotary wing, set to SAS */
+		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
+		current_status.flag_control_attitude_enabled = true;
+		current_status.flag_control_rates_enabled = true;
+	} else {
+
+		/* assuming a fixed wing, set to direct pass-through */
+		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
+		current_status.flag_control_attitude_enabled = false;
+		current_status.flag_control_rates_enabled = false;
+	}
+
 	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) {
diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c
index cd479d40d..646deb62e 100644
--- a/apps/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c
@@ -208,7 +208,13 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 					att_sp.roll_body = 0.3f;
 					att_sp.pitch_body = 0.0f;
 					att_sp.yaw_body = 0;
-					att_sp.thrust = 0.4f;
+					
+					/* 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
 
@@ -250,7 +256,13 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 						//param_get(failsafe_throttle_handle, &failsafe_throttle);
 						att_sp.roll_body = 0.3f;
 						att_sp.pitch_body = 0.0f;
-						att_sp.thrust = 0.4f;
+
+						/* 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;
+						}
 						att_sp.yaw_body = 0;
 
 						// XXX disable yaw control, loiter
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index f184859a3..1b03e8c22 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -253,7 +253,20 @@ mc_thread_main(int argc, char *argv[])
 							param_get(failsafe_throttle_handle, &failsafe_throttle);
 							att_sp.roll_body = 0.0f;
 							att_sp.pitch_body = 0.0f;
-							att_sp.thrust = failsafe_throttle;
+
+							/*
+							 * 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 > 0.2f) {
+								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