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

Jump to bottom

navigator: fix acceptance radius for multicopter #17661

Merged
merged 2 commits into from
Jun 2, 2021
Merged

navigator: fix acceptance radius for multicopter #17661

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e4b41e3
navigator: fix acceptance radius for multicopter
julianoes May 27, 2021
27f4fc7
navigator: protect against 0 acceptance radius
julianoes May 28, 2021
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
14 changes: 10 additions & 4 deletions src/modules/navigator/mission_block.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -277,9 +277,15 @@ MissionBlock::is_mission_item_reached()
_time_wp_reached = now;

} else {
/*normal mission items */

float mission_acceptance_radius = _navigator->get_acceptance_radius();
float acceptance_radius = _navigator->get_acceptance_radius();

// We use the acceptance radius of the mission item if it has been set (not NAN)
// but only for multicopter.
if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can't this logic live inside of get_acceptance_radius()? We could again pass _mission_item.acceptance_radius as an argument.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I actually found the way it was before quite confusing. So get_acceptance_radius() had this overload but the overload was only used from this location, right?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Right. If one anyway only wants to take the acceptance radius from the param then using get_default_acceptance_radius() would be cleaner though no? And always pass _mission_item.acceptance_radius as argument of get_acceptance_radius()?

&& PX4_ISFINITE(_mission_item.acceptance_radius) && _mission_item.acceptance_radius > FLT_EPSILON) {
acceptance_radius = _mission_item.acceptance_radius;
}

/* for vtol back transition calculate acceptance radius based on time and ground speed */
if (_mission_item.vtol_back_transition
Expand All @@ -292,13 +298,13 @@ MissionBlock::is_mission_item_reached()
const float reverse_delay = _navigator->get_vtol_reverse_delay();

if (back_trans_dec > FLT_EPSILON && velocity > FLT_EPSILON) {
mission_acceptance_radius = ((velocity / back_trans_dec / 2) * velocity) + reverse_delay * velocity;
acceptance_radius = ((velocity / back_trans_dec / 2) * velocity) + reverse_delay * velocity;

}

}

if (dist_xy >= 0.0f && dist_xy <= mission_acceptance_radius
if (dist_xy >= 0.0f && dist_xy <= acceptance_radius
&& dist_z <= _navigator->get_altitude_acceptance_radius()) {
_waypoint_position_reached = true;
}
Expand Down