MulticopterLandDetector: refactor maybe_landed condition

removing all early returns

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -79,6 +79,7 @@ MulticopterLandDetector::MulticopterLandDetector()
 	_paramHandle.minThrottle = param_find("MPC_THR_MIN");
 	_paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE");
 	_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
+	_minimum_thrust_8s_hysteresis.set_hysteresis_time_from(false, 8_s);
 }
 
 void MulticopterLandDetector::_update_topics()
@@ -220,48 +221,28 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 	const bool skip_close_to_ground_check = !_dist_bottom_is_observable || !_vehicle_local_position.dist_bottom_valid;
 	_close_to_ground_or_skipped_check = _is_close_to_ground() || skip_close_to_ground_check;
 
-	// When not armed, consider to have ground-contact
-	if (!_armed) {
-		return true;
-	}
-
 	// TODO: we need an accelerometer based check for vertical movement for flying without GPS
-	return _close_to_ground_or_skipped_check && ground_contact && !_horizontal_movement
+	return !_armed ||
-	       && !_vertical_movement;
+	       (_close_to_ground_or_skipped_check && ground_contact
+		&& !_horizontal_movement && !_vertical_movement);
 }
 
 bool MulticopterLandDetector::_get_maybe_landed_state()
 {
-	// When not armed, consider to be maybe-landed
+	hrt_abstime now = hrt_absolute_time();
-	if (!_armed) {
-		return true;
-	}
 
-	const hrt_abstime time_now_us = hrt_absolute_time();
+	float minimum_thrust_threshold{0.f};
 
-	// minimal throttle: initially 10% of throttle range between min and hover
+	if (_flag_control_climb_rate_enabled) {
-	float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f;
+		// 10% of throttle range between min and hover
-
+		minimum_thrust_threshold = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f;
-	// Determine the system min throttle based on flight mode
-	if (!_flag_control_climb_rate_enabled) {
-		sys_min_throttle = (_params.minManThrottle + 0.01f);
-	}
-
-	// Check if thrust output is less than the minimum throttle.
-	if (_actuator_controls_throttle <= sys_min_throttle) {
-		if (_min_thrust_start == 0) {
-			_min_thrust_start = time_now_us;
-		}
 
 	} else {
-		_min_thrust_start = 0;
+		minimum_thrust_threshold = (_params.minManThrottle + 0.01f);
-		return false;
 	}
 
-
+	const bool minimum_thrust_now = _actuator_controls_throttle <= minimum_thrust_threshold;
-	if (_freefall_hysteresis.get_state()) {
+	_minimum_thrust_8s_hysteresis.set_state_and_update(minimum_thrust_now, now);
-		return false;
-	}
 
 	// Next look if vehicle is not rotating (do not consider yaw)
 	float max_rotation_threshold = math::radians(_param_lndmc_rot_max.get());
@@ -272,33 +253,23 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 		max_rotation_threshold *= 2.5f;
 	}
 
-	if (_angular_velocity.xy().norm() > max_rotation_threshold) {
+	_rotational_movement = _angular_velocity.xy().norm() > max_rotation_threshold;
-		_rotational_movement = true;
-		return false;
-
-	} else {
-		_rotational_movement = false;
-	}
 
 	// If vertical velocity is available: ground contact, no thrust, no movement -> landed
-	if (((time_now_us - _vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.v_z_valid) {
+	const bool local_position_updated = (now - _vehicle_local_position.timestamp) < 1_s;
-		return _ground_contact_hysteresis.get_state();
+	const bool vertical_velocity_valid = _vehicle_local_position.v_z_valid;
-	}
+	const bool vertical_estimate = local_position_updated && vertical_velocity_valid;
 
-	// Otherwise, landed if the system has minimum thrust (manual or in failsafe) and no rotation for at least 8 seconds
+	return !_armed ||
-	return (_min_thrust_start > 0) && ((time_now_us - _min_thrust_start) > 8_s);
+	       (minimum_thrust_now && !_freefall_hysteresis.get_state() && !_rotational_movement
+		&& ((vertical_estimate && _ground_contact_hysteresis.get_state())
+		    || (!vertical_estimate && _minimum_thrust_8s_hysteresis.get_state())));
 }
 
 bool MulticopterLandDetector::_get_landed_state()
 {
-	// When not armed, consider to be landed
+	// all maybe_landed conditions need to hold longer
-	if (!_armed) {
+	return !_armed || _maybe_landed_hysteresis.get_state();
-		return true;
-	}
-
-	// if we have maybe_landed, the mc_pos_control goes into idle (thrust_sp = 0.0)
-	// therefore check if all other condition of the landed state remain true
-	return _maybe_landed_hysteresis.get_state();
 }
 
 bool MulticopterLandDetector::_get_ground_effect_state()

src/modules/land_detector/MulticopterLandDetector.h

@@ -119,7 +119,7 @@ private:
 
 	uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED};
 
-	hrt_abstime _min_thrust_start{0};	///< timestamp when minimum trust was applied first
+	systemlib::Hysteresis _minimum_thrust_8s_hysteresis{false};
 
 	bool _in_descend{false};		///< vehicle is commanded to desend
 	bool _horizontal_movement{false};	///< vehicle is moving horizontally