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:
Roman Bapst
2024-09-10 18:44:24 +03:00
committed by GitHub
parent 03aec2e188
commit c94c1ce4d2
10 changed files with 75 additions and 39 deletions
@@ -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