mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-09 07:30:35 +08:00
Navigator: Support straight line mission landings (#23576)
* introduced altitude acceptance radius in position setpoint for fixed wing guidance - allows navigator to explicitly set the altitude acceptance radius - needed for staright line landing support * added ignore_alt_acceptance to position setpoint message to allow guidance logic to ignore altitude error on waypoint - can be useful to prevent loitering at a waypoint within a mission landing sequence * only set altitude acceptance radius to infinity for a waypoint inside a mission landing for fixed wing vehicles * navigator: return altitude acceptance radius from triplet if it's valid * FixedWingPositionControl: check if alt acceptance radius provided in position setpoint is larger 0 --------- Signed-off-by: RomanBapst <bapstroman@gmail.com> Co-authored-by: Alvaro Fernandez <alvaro@auterion.com>
This commit is contained in:
@@ -1029,11 +1029,15 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
|
||||
_current_latitude, _current_longitude, _current_altitude,
|
||||
&dist_xy, &dist_z);
|
||||
|
||||
const float acc_rad_z = (PX4_ISFINITE(pos_sp_curr.alt_acceptance_radius)
|
||||
&& pos_sp_curr.alt_acceptance_radius > FLT_EPSILON) ? pos_sp_curr.alt_acceptance_radius :
|
||||
_param_nav_fw_alt_rad.get();
|
||||
|
||||
// Achieve position setpoint altitude via loiter when laterally close to WP.
|
||||
// Detect if system has switchted into a Loiter before (check _position_sp_type), and in that
|
||||
// case remove the dist_xy check (not switch out of Loiter until altitude is reached).
|
||||
if ((!_vehicle_status.in_transition_mode) && (dist >= 0.f)
|
||||
&& (dist_z > _param_nav_fw_alt_rad.get())
|
||||
&& (dist_z > acc_rad_z)
|
||||
&& (dist_xy < acc_rad || _position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER)) {
|
||||
|
||||
// SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER
|
||||
|
||||
Reference in New Issue
Block a user