From c94c1ce4d2712fa8af892eaeeae3a6aa2f859961 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 10 Sep 2024 18:44:24 +0300 Subject: [PATCH] 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 Co-authored-by: Alvaro Fernandez --- msg/PositionSetpoint.msg | 3 +- .../FixedwingPositionControl.cpp | 6 ++- src/modules/navigator/mission.cpp | 42 ++++--------------- src/modules/navigator/mission.h | 1 - src/modules/navigator/mission_base.cpp | 36 ++++++++++++++++ src/modules/navigator/mission_base.h | 3 ++ src/modules/navigator/mission_block.cpp | 4 ++ src/modules/navigator/navigator_main.cpp | 7 +++- .../navigator/rtl_direct_mission_land.cpp | 7 ++++ src/modules/navigator/rtl_mission_fast.cpp | 5 +++ 10 files changed, 75 insertions(+), 39 deletions(-) diff --git a/msg/PositionSetpoint.msg b/msg/PositionSetpoint.msg index 035d35205ee2..51bfaa3da0b9 100644 --- a/msg/PositionSetpoint.msg +++ b/msg/PositionSetpoint.msg @@ -30,7 +30,8 @@ bool loiter_direction_counter_clockwise # loiter direction is clockwise by defau float32 loiter_orientation # Orientation of the major axis with respect to true north in rad [-pi,pi) uint8 loiter_pattern # loitern pattern to follow -float32 acceptance_radius # navigation acceptance_radius if we're doing waypoint navigation +float32 acceptance_radius # horizontal acceptance_radius (meters) +float32 alt_acceptance_radius # vertical acceptance radius, only used for fixed wing guidance, NAN = let guidance choose (meters) 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) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 7858fe2c1c5f..00e7869c6949 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -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 diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ea7dae675217..cb8f07fa0e4f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -91,41 +91,6 @@ Mission::on_activation() MissionBase::on_activation(); } -bool -Mission::isLanding() -{ - if (get_land_start_available()) { - static constexpr size_t max_num_next_items{1u}; - int32_t next_mission_items_index[max_num_next_items]; - size_t num_found_items; - - getNextPositionItems(_mission.land_start_index + 1, next_mission_items_index, num_found_items, max_num_next_items); - - // vehicle is currently landing if - // mission valid, still flying, and in the landing portion of mission (past land start marker) - bool on_landing_stage = (num_found_items > 0U) && _mission.current_seq > next_mission_items_index[0U]; - - // special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the - // distance to the WP is below the loiter radius + acceptance. - if ((num_found_items > 0U) && _mission.current_seq == next_mission_items_index[0U] - && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { - const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); - - // consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case. - const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius) - && fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) : - _navigator->get_loiter_radius(); - - on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs); - } - - return _navigator->get_mission_result()->valid && on_landing_stage; - - } else { - return false; - } -} bool Mission::set_current_mission_index(uint16_t index) @@ -244,6 +209,13 @@ void Mission::setActiveMissionItems() mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); } + // prevent fixed wing lateral guidance from loitering at a waypoint as part of a mission landing if the altitude + // is not achieved. + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && isLanding() && + _mission_item.nav_cmd == NAV_CMD_WAYPOINT) { + pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX; + } + // 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.h b/src/modules/navigator/mission.h index 7299e065e584..8a24ac4dc0d3 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -67,7 +67,6 @@ class Mission : public MissionBase uint16_t get_land_start_index() const { return _mission.land_start_index; } bool get_land_start_available() const { return hasMissionLandStart(); } - bool isLanding(); private: diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index cee250095be1..37f104dea2b7 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -367,6 +367,42 @@ MissionBase::on_active() updateAltToAvoidTerrainCollisionAndRepublishTriplet(_mission_item); } +bool +MissionBase::isLanding() +{ + if (hasMissionLandStart() && (_mission.current_seq > _mission.land_start_index)) { + static constexpr size_t max_num_next_items{1u}; + int32_t next_mission_items_index[max_num_next_items]; + size_t num_found_items; + + getNextPositionItems(_mission.land_start_index + 1, next_mission_items_index, num_found_items, max_num_next_items); + + // vehicle is currently landing if + // mission valid, still flying, and in the landing portion of mission (past land start marker) + bool on_landing_stage = (num_found_items > 0U) && _mission.current_seq > next_mission_items_index[0U]; + + // special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the + // distance to the WP is below the loiter radius + acceptance. + if ((num_found_items > 0U) && _mission.current_seq == next_mission_items_index[0U] + && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { + const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + + // consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case. + const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius) + && fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) : + _navigator->get_loiter_radius(); + + on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs); + } + + return _navigator->get_mission_result()->valid && on_landing_stage; + + } else { + return false; + } +} + void MissionBase::update_mission() { if (_mission.count == 0u || !_is_current_planned_mission_item_valid || !isMissionValid()) { diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index f819b5c0e51a..1a6cbf977534 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -73,6 +73,8 @@ class MissionBase : public MissionBlock, public ModuleParams virtual void on_activation() override; virtual void on_active() override; + bool isLanding(); + protected: /** @@ -321,6 +323,7 @@ class MissionBase : public MissionBlock, public ModuleParams */ void setMissionIndex(int32_t index); + bool _is_current_planned_mission_item_valid{false}; /**< Flag indicating if the currently loaded mission item is valid*/ bool _mission_has_been_activated{false}; /**< Flag indicating if the mission has been activated*/ bool _mission_checked{false}; /**< Flag indicating if the mission has been checked by the mission validator*/ diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 2659ad95cc57..84d95ae5a310 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -673,6 +673,10 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi sp->acceptance_radius = _navigator->get_default_acceptance_radius(); } + // by default, FW guidance logic will take alt acceptance from NAV_FW_ALT_RAD, in some special cases + // we override it after this + sp->alt_acceptance_radius = NAN; + sp->cruising_speed = _navigator->get_cruising_speed(); sp->cruising_throttle = _navigator->get_cruising_throttle(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dbf3324b867e..9a9f46e96282 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1116,9 +1116,14 @@ float Navigator::get_default_acceptance_radius() float Navigator::get_altitude_acceptance_radius() { if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + + const position_setpoint_s &curr_sp = get_position_setpoint_triplet()->current; const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next; - if (!force_vtol() && next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) { + if ((PX4_ISFINITE(curr_sp.alt_acceptance_radius) && curr_sp.alt_acceptance_radius > FLT_EPSILON)) { + return curr_sp.alt_acceptance_radius; + + } else if (!force_vtol() && next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) { // Use separate (tighter) altitude acceptance for clean altitude starting point before FW landing return _param_nav_fw_altl_rad.get(); diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index 5f55b92affd8..bc6df21a49c3 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -212,6 +212,13 @@ 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 (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && MissionBase::isLanding() + && _mission_item.nav_cmd == NAV_CMD_WAYPOINT) { + pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX; + } } issue_command(_mission_item); diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp index 0bcafd94e8fb..d3e07a36ddc3 100644 --- a/src/modules/navigator/rtl_mission_fast.cpp +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -168,6 +168,11 @@ void RtlMissionFast::setActiveMissionItems() } mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && isLanding() && + _mission_item.nav_cmd == NAV_CMD_WAYPOINT) { + pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX; + } } issue_command(_mission_item);