Merge branch 'master' into offboard2

1 files changed, 44 insertions, 4 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 96735d25e..90efd5115 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -456,6 +456,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 					/* AUTO */
 					main_res = main_state_transition(status, MAIN_STATE_AUTO);
 
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
+					/* ACRO */
+					main_res = main_state_transition(status, MAIN_STATE_ACRO);
+
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
 					/* OFFBOARD */
 					main_res = main_state_transition(status, MAIN_STATE_OFFBOARD);
@@ -658,7 +662,8 @@ int commander_thread_main(int argc, char *argv[])
 	main_states_str[1] = "ALTCTL";
 	main_states_str[2] = "POSCTL";
 	main_states_str[3] = "AUTO";
-	main_states_str[4] = "OFFBOARD";
+	main_states_str[4] = "ACRO";
+	main_states_str[5] = "OFFBOARD";
 
 	char *arming_states_str[ARMING_STATE_MAX];
 	arming_states_str[0] = "INIT";
@@ -1018,7 +1023,26 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* update condition_local_position_valid and condition_local_altitude_valid */
-		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed);
+		/* hysteresis for EPH */
+		bool local_eph_good;
+
+		if (status.condition_global_position_valid) {
+			if (local_position.eph > eph_epv_threshold * 2.0f) {
+				local_eph_good = false;
+
+			} else {
+				local_eph_good = true;
+			}
+
+		} else {
+			if (local_position.eph < eph_epv_threshold) {
+				local_eph_good = true;
+
+			} else {
+				local_eph_good = false;
+			}
+		}
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed);
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
 		static bool published_condition_landed_fw = false;
@@ -1193,7 +1217,7 @@ int commander_thread_main(int argc, char *argv[])
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing &&
 			    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
-			    (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
+			    (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) &&
 			    sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
@@ -1650,7 +1674,12 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 		break;
 
 	case SWITCH_POS_OFF:		// MANUAL
-		res = main_state_transition(status, MAIN_STATE_MANUAL);
+		if (sp_man->acro_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status, MAIN_STATE_ACRO);
+
+		} else {
+			res = main_state_transition(status, MAIN_STATE_MANUAL);
+		}
 		// TRANSITION_DENIED is not possible here
 		break;
 
@@ -1763,6 +1792,17 @@ set_control_mode()
 			navigator_enabled = true;
 			break;
 
+		case MAIN_STATE_ACRO:
+			control_mode.flag_control_manual_enabled = true;
+			control_mode.flag_control_auto_enabled = false;
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
 		case MAIN_STATE_OFFBOARD:
 			control_mode.flag_control_manual_enabled = false;
 			control_mode.flag_control_auto_enabled = false;