From 473b471fb6f5b57f6e81ad802594578ea72d0ad3 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 26 Oct 2022 13:53:58 +0200 Subject: [PATCH] Navigator: add guards for using mission_item.loiter_radius only if finite and >FLT_EPS Signed-off-by: Silvan Fuhrer --- src/modules/navigator/mission.cpp | 4 +++- src/modules/navigator/mission_block.cpp | 13 +++++++++---- src/modules/navigator/navigator_main.cpp | 2 +- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 5d6c54c6f5..04f02358f7 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -443,7 +443,9 @@ Mission::find_mission_land_start() _landing_start_lon = missionitem.lon; _landing_start_alt = missionitem.altitude_is_relative ? missionitem.altitude + _navigator->get_home_position()->alt : missionitem.altitude; - _landing_loiter_radius = missionitem.loiter_radius; + _landing_loiter_radius = (PX4_ISFINITE(missionitem.loiter_radius) + && fabsf(missionitem.loiter_radius) > FLT_EPSILON) ? fabsf(missionitem.loiter_radius) : + _navigator->get_loiter_radius(); _land_start_available = true; } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 15319a3af4..7a996efb92 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -191,6 +191,11 @@ MissionBlock::is_mission_item_reached_or_completed() const float mission_item_altitude_amsl = get_absolute_altitude_for_item(_mission_item); + // 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(); + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, mission_item_altitude_amsl, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, @@ -254,7 +259,7 @@ MissionBlock::is_mission_item_reached_or_completed() */ // check if within loiter radius around wp, if yes then set altitude sp to mission item - if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius)) + if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs) && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; @@ -276,7 +281,7 @@ MissionBlock::is_mission_item_reached_or_completed() &dist_xy, &dist_z); // check if within loiter radius around wp, if yes then set altitude sp to mission item - if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius)) + if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs) && dist_z <= _navigator->get_altitude_acceptance_radius()) { curr_sp->alt = mission_item_altitude_amsl; @@ -284,7 +289,7 @@ MissionBlock::is_mission_item_reached_or_completed() _navigator->set_position_setpoint_triplet_updated(); } - } else if (dist >= 0.f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius)) + } else if (dist >= 0.f && dist_xy <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs) && dist_z <= _navigator->get_altitude_acceptance_radius()) { // loitering, check if new altitude is reached, while still also having check on position @@ -509,7 +514,7 @@ MissionBlock::is_mission_item_reached_or_completed() float bearing = get_bearing_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon); // calculate (positive) angle between current bearing vector (orbit center to next waypoint) and vector pointing to tangent exit location - const float ratio = math::min(fabsf(_mission_item.loiter_radius / range), 1.0f); + const float ratio = math::min(fabsf(curr_sp.loiter_radius / range), 1.0f); float inner_angle = acosf(ratio); // Compute "ideal" tangent origin diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8bba5d4131..fa5ac3bfde 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -344,7 +344,7 @@ void Navigator::run() } if (only_alt_change_requested) { - if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > 0) { + if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) { rep->current.loiter_radius = curr->current.loiter_radius;