commander: don't notify user about rejected disarm to not confuse, flag_control_altitude_enabled added, flags values fixed

3 files changed, 33 insertions, 31 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 25c5a84ea..1978d8782 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1398,30 +1398,18 @@ int commander_thread_main(int argc, char *argv[])
 					}
 				}
 
-				/*
-				* Check if left stick is in lower left position --> switch to standby state.
-				* Do this only for multirotors, not for fixed wing aircraft.
-				*/
-				if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) {
-
+				/* Check if left stick is in lower left position and we're in manual mode --> switch to standby state.
+				 * Do this only for multirotors, not for fixed wing aircraft.
+				 * TODO allow disarm when landed
+				 */
+				if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f &&
+					control_mode.flag_control_manual_enabled &&
+					((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
+					 (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
+					 (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) {
 					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-
-						if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-						 (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-						 (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
-						) {
-							if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) {
-								mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first");
-								tune_negative();
-							} else {
-								arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd);
-								tune_positive();
-							}
-							
-						} else {
-							mavlink_log_critical(mavlink_fd, "DISARM not allowed");
-							tune_negative();
-						}
+						arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd);
+						tune_positive();
 						stick_off_counter = 0;
 
 					} else {
@@ -1430,9 +1418,8 @@ int commander_thread_main(int argc, char *argv[])
 					}
 				}
 
-				/* check if left stick is in lower right position and we're in manual --> arm */
-				if (sp_man.yaw > STICK_ON_OFF_LIMIT &&
-					sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+				/* check if left stick is in lower right position and we're in manual mode --> arm */
+				if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
 					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 						arming_state_transition(status_pub, &current_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd);
 						stick_on_counter = 0;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 60ab01536..4a7aa66b7 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -192,6 +192,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 					control_mode->flag_control_attitude_enabled = false;
 					control_mode->flag_control_velocity_enabled = false;
 					control_mode->flag_control_position_enabled = false;
+					control_mode->flag_control_altitude_enabled = false;
 					control_mode->flag_control_manual_enabled = false;
 				}
 				break;
@@ -214,6 +215,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 						control_mode->flag_control_attitude_enabled = true;
 						control_mode->flag_control_velocity_enabled = false;
 						control_mode->flag_control_position_enabled = false;
+						control_mode->flag_control_altitude_enabled = false;
 						control_mode->flag_control_manual_enabled = true;
 					}
 				}
@@ -231,6 +233,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 					control_mode->flag_control_attitude_enabled = true;
 					control_mode->flag_control_velocity_enabled = false;
 					control_mode->flag_control_position_enabled = false;
+					control_mode->flag_control_altitude_enabled = false;
 					control_mode->flag_control_manual_enabled = true;
 				}
 				break;
@@ -258,7 +261,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 						control_mode->flag_control_attitude_enabled = true;
 						control_mode->flag_control_velocity_enabled = true;
 						control_mode->flag_control_position_enabled = false;
-						control_mode->flag_control_manual_enabled = false;
+						control_mode->flag_control_altitude_enabled = true;
+						control_mode->flag_control_manual_enabled = true;
 					}
 				}
 				break;
@@ -290,7 +294,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 						control_mode->flag_control_attitude_enabled = true;
 						control_mode->flag_control_velocity_enabled = true;
 						control_mode->flag_control_position_enabled = false;
-						control_mode->flag_control_manual_enabled = false;
+						control_mode->flag_control_altitude_enabled = true;
+						control_mode->flag_control_manual_enabled = true;
 					}
 				}
 				break;
@@ -321,8 +326,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 						control_mode->flag_control_rates_enabled = true;
 						control_mode->flag_control_attitude_enabled = true;
 						control_mode->flag_control_velocity_enabled = true;
-						control_mode->flag_control_position_enabled = false;
-						control_mode->flag_control_manual_enabled = false;
+						control_mode->flag_control_position_enabled = true;
+						control_mode->flag_control_altitude_enabled = true;
+						control_mode->flag_control_manual_enabled = true;
 					}
 				}
 				break;
@@ -353,7 +359,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 						control_mode->flag_control_attitude_enabled = true;
 						control_mode->flag_control_velocity_enabled = true;
 						control_mode->flag_control_position_enabled = false;
-						control_mode->flag_control_manual_enabled = false;
+						control_mode->flag_control_altitude_enabled = true;
+						control_mode->flag_control_manual_enabled = true;
 					}
 				}
 				break;
@@ -382,6 +389,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 						control_mode->flag_control_attitude_enabled = true;
 						control_mode->flag_control_velocity_enabled = true;
 						control_mode->flag_control_position_enabled = true;
+						control_mode->flag_control_altitude_enabled = true;
 						control_mode->flag_control_manual_enabled = false;
 					}
 				}
@@ -405,6 +413,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 						control_mode->flag_control_attitude_enabled = true;
 						control_mode->flag_control_velocity_enabled = true;
 						control_mode->flag_control_position_enabled = true;
+						control_mode->flag_control_altitude_enabled = true;
 						control_mode->flag_control_manual_enabled = false;
 					}
 				}
@@ -420,6 +429,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 					control_mode->flag_control_attitude_enabled = true;
 					control_mode->flag_control_velocity_enabled = true;
 					control_mode->flag_control_position_enabled = true;
+					control_mode->flag_control_altitude_enabled = true;
 					control_mode->flag_control_manual_enabled = false;
 				}
 				break;
@@ -447,6 +457,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 						control_mode->flag_control_attitude_enabled = true;
 						control_mode->flag_control_velocity_enabled = true;
 						control_mode->flag_control_position_enabled = true;
+						control_mode->flag_control_altitude_enabled = true;
 						control_mode->flag_control_manual_enabled = false;
 					}
 				}
@@ -472,6 +483,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 						control_mode->flag_control_attitude_enabled = true;
 						control_mode->flag_control_velocity_enabled = true;
 						control_mode->flag_control_position_enabled = true;
+						control_mode->flag_control_altitude_enabled = true;
 						control_mode->flag_control_manual_enabled = false;
 					}
 				}
@@ -500,6 +512,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 						control_mode->flag_control_attitude_enabled = true;
 						control_mode->flag_control_velocity_enabled = true;
 						control_mode->flag_control_position_enabled = true;
+						control_mode->flag_control_altitude_enabled = true;
 						control_mode->flag_control_manual_enabled = false;
 					}
 				}
@@ -524,6 +537,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current
 						control_mode->flag_control_attitude_enabled = true;
 						control_mode->flag_control_velocity_enabled = true;
 						control_mode->flag_control_position_enabled = true;
+						control_mode->flag_control_altitude_enabled = true;
 						control_mode->flag_control_manual_enabled = false;
 					}
 				}
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 8481a624f..d83eb45d9 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -77,6 +77,7 @@ struct vehicle_control_mode_s
 	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
 	bool flag_control_velocity_enabled;		/**< true if speed (implies direction) is controlled */
 	bool flag_control_position_enabled;		/**< true if position is controlled */
+	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
 
 	bool failsave_lowlevel;				/**< Set to true if low-level failsafe mode is enabled */
 	bool failsave_highlevel;			/**< Set to true if high-level failsafe mode is enabled */