fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
When not specified by navigator, the center of the landing orbit is set
to the current position when landing is triggered.
This commit is contained in:
Mathieu Bresciani 2026-01-08 14:44:00 +01:00 committed by GitHub
parent 7c318a3296
commit c71e2d41d6
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 43 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
src/modules/fw_mode_manager/FixedWingModeManager.cpp
View File

@ -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,6 +1614,12 @@ 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();
@ -1601,12 +1627,11 @@ FixedWingModeManager::control_auto_landing_circular(const hrt_abstime &now, cons
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;

1
src/modules/fw_mode_manager/FixedWingModeManager.hpp
View File

@ -319,6 +319,7 @@ private:
// 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};