Fixedwing: Fix circular landing when global origin is not set (#26223)

When not specified by navigator, the center of the landing orbit is set
to the current position when landing is triggered.

Author: bresch

src/modules/fw_mode_manager/FixedWingModeManager.cpp

@@ -388,11 +388,9 @@ FixedWingModeManager::set_control_mode_current(const hrt_abstime &now)
 		}
 
 	} else if ((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled)
-		   && (_position_setpoint_current_valid
-		       || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
+		   && _position_setpoint_current_valid) {
 
-		// Enter this mode only if the current waypoint has valid 3D position setpoints or is of type IDLE.
-		// A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO.
+		// Enter this mode only if the current waypoint has valid 3D position setpoints.
 
 		if (doing_backtransition) {
 			_control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_LINE_FOLLOW;
@@ -433,6 +431,28 @@ FixedWingModeManager::set_control_mode_current(const hrt_abstime &now)
 			_control_mode_current = FW_POSCTRL_MODE_AUTO;
 		}
 
+	} else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled
+		   && (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
+
+		// A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO.
+		if (doing_backtransition) {
+			_control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD;
+
+		} else {
+			_control_mode_current = FW_POSCTRL_MODE_AUTO;
+		}
+
+	} else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled
+		   && (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) {
+
+		if (doing_backtransition) {
+			_control_mode_current = FW_POSCTRL_MODE_TRANSITION_TO_HOVER_HEADING_HOLD;
+
+		} else {
+			// Only circular landing are supported when LAND is sent without valid position
+			_control_mode_current = FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR;
+		}
+
 	} else if (_control_mode.flag_control_auto_enabled
 		   && _control_mode.flag_control_climb_rate_enabled
 		   && _control_mode.flag_armed // only enter this modes if armed, as pure failsafe modes
@@ -1594,19 +1614,24 @@ void
 FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, const float control_interval,
 		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
 {
+	if (_time_started_landing == 0) {
+		// save time at which we started landing and reset landing abort status
+		reset_landing_state();
+		_time_started_landing = now;
+	}
+
 	const float airspeed_land = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() :
 				    _param_fw_airspd_min.get();
 
 	_ctrl_configuration_handler.setEnforceLowHeightCondition(true);
 
 
 	const Vector2f local_position{_local_pos.x, _local_pos.y};
-	Vector2f local_landing_orbit_center = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
 
-	if (_time_started_landing == 0) {
-		// save time at which we started landing and reset landing abort status
-		reset_landing_state();
-		_time_started_landing = now;
+	if (!_local_landing_orbit_center.isAllFinite()) {
+		_local_landing_orbit_center = _position_setpoint_current_valid
+					      ? _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon)
+					      : local_position;
 	}
 
 	const bool abort_on_terrain_timeout = checkLandingAbortBitMask(_param_fw_lnd_abort.get(),
@@ -1642,7 +1667,7 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons
 						      1.0f);
 
 		/* lateral guidance first, because npfg will adjust the airspeed setpoint if necessary */
-		const DirectionalGuidanceOutput sp = navigateLoiter(local_landing_orbit_center, local_position, loiter_radius,
+		const DirectionalGuidanceOutput sp = navigateLoiter(_local_landing_orbit_center, local_position, loiter_radius,
 						     loiter_direction_ccw,
 						     ground_speed, _wind_vel);
 		fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
@@ -1700,7 +1725,7 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons
 	} else {
 
 		// follow the glide slope
-		const DirectionalGuidanceOutput sp = navigateLoiter(local_landing_orbit_center, local_position, loiter_radius,
+		const DirectionalGuidanceOutput sp = navigateLoiter(_local_landing_orbit_center, local_position, loiter_radius,
 						     loiter_direction_ccw,
 						     ground_speed, _wind_vel);
 		fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
@@ -2285,6 +2310,7 @@ FixedWingModeManager::reset_landing_state()
 
 	_flare_states = FlareStates{};
 
+	_local_landing_orbit_center.setNaN();
 	_lateral_touchdown_position_offset = 0.0f;
 
 	_last_time_terrain_alt_was_valid = 0;
@@ -2299,6 +2325,10 @@ FixedWingModeManager::reset_landing_state()
 
 float FixedWingModeManager::getMaxRollAngleNearGround(const float altitude, const float terrain_altitude) const
 {
+	if (!PX4_ISFINITE(altitude) || !PX4_ISFINITE(terrain_altitude)) {
+		return math::radians(_param_fw_r_lim.get());
+	}
+
 	// we want the wings level when at the wing height above ground
 	const float height_above_ground = math::max(altitude - (terrain_altitude + _param_fw_wing_height.get()), 0.0f);
 
@@ -2424,7 +2454,7 @@ FixedWingModeManager::getLandingTerrainAltitudeEstimate(const hrt_abstime &now,
 
 		if (_local_pos.dist_bottom_valid) {
 
-			const float terrain_estimate = _local_pos.ref_alt + -_local_pos.z - _local_pos.dist_bottom;
+			const float terrain_estimate = _reference_altitude + -_local_pos.z - _local_pos.dist_bottom;
 			_last_valid_terrain_alt_estimate = terrain_estimate;
 			_last_time_terrain_alt_was_valid = now;
 

src/modules/fw_mode_manager/FixedWingModeManager.hpp

@@ -319,6 +319,7 @@ class FixedWingModeManager final : public ModuleBase<FixedWingModeManager>, publ
 	// orbit to altitude only when the aircraft has entered the final *straight approach.
 	hrt_abstime _time_started_landing{0};
 
+	Vector2f _local_landing_orbit_center{NAN, NAN};
 	// [m] lateral touchdown position offset manually commanded during landing
 	float _lateral_touchdown_position_offset{0.0f};