diff --git a/msg/PositionSetpoint.msg b/msg/PositionSetpoint.msg index 035d35205e..20dd9767c2 100644 --- a/msg/PositionSetpoint.msg +++ b/msg/PositionSetpoint.msg @@ -35,3 +35,4 @@ float32 acceptance_radius # navigation acceptance_radius if we're doing waypoi float32 cruising_speed # the generally desired cruising speed (not a hard constraint) bool gliding_enabled # commands the vehicle to glide if the capability is available (fixed wing only) float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint), only has an effect for rover +bool ignore_alt_acceptance # can be set to avoid lateral guidance to loiter at waypoint in order to achieve altitude acceptance diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index a1286db402..51a45eff9d 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -1023,11 +1023,17 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp _current_latitude, _current_longitude, _current_altitude, &dist_xy, &dist_z); + float acc_rad_z = _param_nav_fw_alt_rad.get(); + + if (pos_sp_curr.ignore_alt_acceptance) { + acc_rad_z = INFINITY; + } + // 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 diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 06b4151bdb..ebef68dc51 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -227,6 +227,12 @@ void Mission::setActiveMissionItems() mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); } + // prevent lateral guidance from loitering at a waypoint as part of a mission landing if the altitude + // is not achieved. + if (isLanding() && _mission_item.nav_cmd == NAV_CMD_WAYPOINT) { + pos_sp_triplet->current.ignore_alt_acceptance = true; + } + // Allow a rotary wing vehicle to decelerate before reaching a wp with a hold time or a timeout // This is done by setting the position triplet's next position's valid flag to false, // which makes the FlightTask disregard the next position diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 17dd3e620c..ccf4a7dc86 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -359,6 +359,12 @@ MissionBlock::is_mission_item_reached_or_completed() } + // prevent lateral guidance from loitering at a waypoint as part of a mission landing if the altitude + // is not achieved. + if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _navigator->on_mission_landing()) { + alt_acc_rad_m = INFINITY; + } + bool passed_curr_wp = false; if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 03a1ea621e..674570bff2 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -176,6 +176,8 @@ public: float get_loiter_radius() { return _param_nav_loiter_rad.get(); } + bool on_mission_landing() { return _mission.isLanding(); } + /** * Returns the default acceptance radius defined by the parameter */ diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index df6674e34a..96867e6fb6 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -213,6 +213,12 @@ void RtlDirectMissionLand::setActiveMissionItems() } mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + + // prevent lateral guidance from loitering at a waypoint as part of a mission landing if the altitude + // is not achieved. + if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT) { + pos_sp_triplet->current.ignore_alt_acceptance = true; + } } issue_command(_mission_item);