commander: disarm from safety relax land detector timeout

- ensure land detector is updated before safety is checked

src/modules/commander/Commander.cpp

@@ -1530,6 +1530,41 @@ Commander::run()
 			}
 		}
 
+		/* Update land detector */
+		if (_land_detector_sub.updated()) {
+			const bool was_landed = _land_detector.landed;
+			_land_detector_sub.copy(&_land_detector);
+
+			// Only take actions if armed
+			if (armed.armed) {
+				if (!was_landed && _land_detector.landed) {
+					mavlink_log_info(&mavlink_log_pub, "Landing detected");
+
+				} else if (was_landed && !_land_detector.landed) {
+					mavlink_log_info(&mavlink_log_pub, "Takeoff detected");
+					_time_at_takeoff = hrt_absolute_time();
+					_have_taken_off_since_arming = true;
+
+					// Set all position and velocity test probation durations to takeoff value
+					// This is a larger value to give the vehicle time to complete a failsafe landing
+					// if faulty sensors cause loss of navigation shortly after takeoff.
+					_gpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
+					_lpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
+					_lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
+
+					// automatically set or update home position
+					if (!_home_pub.get().manual_home) {
+						// set the home position when taking off, but only if we were previously disarmed
+						// and at least 500 ms from commander start spent to avoid setting home on in-air restart
+						if (_should_set_home_on_takeoff && (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
+							_should_set_home_on_takeoff = false;
+							set_home_position();
+						}
+					}
+				}
+			}
+		}
+
 		/* update safety topic */
 		if (_safety_sub.updated()) {
 			const bool previous_safety_off = _safety.safety_off;
@@ -1540,12 +1575,9 @@ Commander::run()
 
 					bool safety_disarm_allowed = (status.hil_state == vehicle_status_s::HIL_STATE_OFF);
 
-					// if land detector is available then prevent disarming via safety button if not landed
+					// prevent disarming via safety button if not landed
-					if (hrt_elapsed_time(&_land_detector.timestamp) < 1_s) {
+					if (hrt_elapsed_time(&_land_detector.timestamp) < 10_s) {
-
+						if (!_land_detector.landed) {
-						bool maybe_landing = (_land_detector.landed || _land_detector.maybe_landed);
-
-						if (!maybe_landing) {
 							safety_disarm_allowed = false;
 						}
 					}
@@ -1628,32 +1660,6 @@ Commander::run()
 
 		estimator_check(status_flags);
 
-		/* Update land detector */
-		if (_land_detector_sub.updated()) {
-			_was_landed = _land_detector.landed;
-			_land_detector_sub.copy(&_land_detector);
-
-			// Only take actions if armed
-			if (armed.armed) {
-				if (_was_landed != _land_detector.landed) {
-					if (_land_detector.landed) {
-						mavlink_log_info(&mavlink_log_pub, "Landing detected");
-
-					} else {
-						mavlink_log_info(&mavlink_log_pub, "Takeoff detected");
-						_have_taken_off_since_arming = true;
-
-						// Set all position and velocity test probation durations to takeoff value
-						// This is a larger value to give the vehicle time to complete a failsafe landing
-						// if faulty sensors cause loss of navigation shortly after takeoff.
-						_gpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
-						_lpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
-						_lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
-					}
-				}
-			}
-		}
-
 
 		// Auto disarm when landed or kill switch engaged
 		if (armed.armed) {
@@ -2278,14 +2284,6 @@ Commander::run()
 		if (!_home_pub.get().manual_home) {
 			const vehicle_local_position_s &local_position = _local_position_sub.get();
 
-			// set the home position when taking off, but only if we were previously disarmed
-			// and at least 500 ms from commander start spent to avoid setting home on in-air restart
-			if (_should_set_home_on_takeoff && _was_landed && !_land_detector.landed &&
-			    (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
-				_should_set_home_on_takeoff = false;
-				set_home_position();
-			}
-
 			if (!armed.armed) {
 				if (status_flags.condition_home_position_valid) {
 					if (_land_detector.landed && local_position.xy_valid && local_position.z_valid) {
@@ -4137,15 +4135,10 @@ void Commander::estimator_check(const vehicle_status_flags_s &vstatus_flags)
 		if (run_quality_checks && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 
 			if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
-				// reset flags and timer
+				// reset flags
-				_time_at_takeoff = hrt_absolute_time();
 				_nav_test_failed = false;
 				_nav_test_passed = false;
 
-			} else if (_land_detector.landed) {
-				// record time of takeoff
-				_time_at_takeoff = hrt_absolute_time();
-
 			} else {
 				// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
 				const bool sufficient_time = (hrt_elapsed_time(&_time_at_takeoff) > 30_s);

src/modules/commander/Commander.hpp

@@ -365,7 +365,6 @@ private:
 
 	bool		_status_changed{true};
 	bool		_arm_tune_played{false};
-	bool		_was_landed{true};
 	bool		_was_armed{false};
 	bool		_failsafe_old{false};	///< check which state machines for changes, clear "changed" flag
 	bool		_have_taken_off_since_arming{false};