Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Navigator: Set altitude acceptance radius to infinity when moving to land point after transition #24115

Merged
merged 2 commits into from
Dec 20, 2024
Merged

Navigator: Set altitude acceptance radius to infinity when moving to land point after transition #24115

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
aeae546
navigator: set alt acceptance radius to infinity
RomanBapst Dec 16, 2024
b9b82ec
added check for vtol
RomanBapst Dec 20, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 8 additions & 2 deletions src/modules/navigator/mission.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -211,8 +211,14 @@ void Mission::setActiveMissionItems()

// 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) {
const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
&& isLanding() &&
_mission_item.nav_cmd == NAV_CMD_WAYPOINT;
const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type ==
RomanBapst marked this conversation as resolved.
Show resolved Hide resolved
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol &&
new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;

if (fw_on_mission_landing || mc_landing_after_transition) {
pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;
}

Expand Down
9 changes: 7 additions & 2 deletions src/modules/navigator/navigator_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1145,8 +1145,13 @@ float Navigator::get_altitude_acceptance_radius()

const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();

if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp)
&& pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) {
const position_setpoint_s &curr_sp = get_position_setpoint_triplet()->current;

if (PX4_ISFINITE(curr_sp.alt_acceptance_radius) && curr_sp.alt_acceptance_radius > FLT_EPSILON) {
alt_acceptance_radius = curr_sp.alt_acceptance_radius;

} else if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp)
sfuhrer marked this conversation as resolved.
Show resolved Hide resolved
&& pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) {
alt_acceptance_radius = pos_ctrl_status.altitude_acceptance;
}

Expand Down
1 change: 1 addition & 0 deletions src/modules/navigator/rtl_direct.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -355,6 +355,7 @@ void RtlDirect::set_rtl_item()
pos_yaw_sp.alt = loiter_altitude;
pos_yaw_sp.yaw = !_param_wv_en.get() ? _destination.yaw : NAN; // set final yaw if weather vane is disabled

altitude_acceptance_radius = FLT_MAX;
setMoveToPositionMissionItem(_mission_item, pos_yaw_sp);
_navigator->reset_position_setpoint(pos_sp_triplet->previous);

Expand Down
10 changes: 8 additions & 2 deletions src/modules/navigator/rtl_direct_mission_land.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -215,8 +215,14 @@ void RtlDirectMissionLand::setActiveMissionItems()

// 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) {
const bool fw_on_mission_landing = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
&& isLanding() &&
_mission_item.nav_cmd == NAV_CMD_WAYPOINT;
const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type ==
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol &&
new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;

if (fw_on_mission_landing || mc_landing_after_transition) {
pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;
}
}
Expand Down
7 changes: 7 additions & 0 deletions src/modules/navigator/rtl_mission_fast_reverse.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -155,6 +155,13 @@ void RtlMissionFastReverse::setActiveMissionItems()
}

mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
const bool mc_landing_after_transition = _vehicle_status_sub.get().vehicle_type ==
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol &&
new_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;

if (mc_landing_after_transition) {
pos_sp_triplet->current.alt_acceptance_radius = FLT_MAX;
}
}

issue_command(_mission_item);
Expand Down
Loading