From c8189b9c234173e8b113bbfb7456650a3b0102bd Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 29 Jun 2017 12:08:11 -0400 Subject: [PATCH] navigator mission_block check range for loiter exit - fixes #7510 --- src/modules/navigator/mission_block.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 7aa9bad7a4..4d113698a6 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -363,31 +363,32 @@ MissionBlock::is_mission_item_reached() if ((get_time_inside(_mission_item) < FLT_EPSILON) || (now - _time_first_inside_orbit >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) { + position_setpoint_s &curr_sp = _navigator->get_position_setpoint_triplet()->current; + const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next; + + const float range = get_distance_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon); + // exit xtrack location - if (_mission_item.loiter_exit_xtrack && + // reset lat/lon of loiter waypoint so vehicle follows a tangent + if (_mission_item.loiter_exit_xtrack && next_sp.valid && PX4_ISFINITE(range) && (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { - // reset lat/lon of loiter waypoint so vehicle follows a tangent - struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; - const struct position_setpoint_s *next_sp = &_navigator->get_position_setpoint_triplet()->next; - float range = get_distance_to_next_waypoint(curr_sp->lat, curr_sp->lon, next_sp->lat, next_sp->lon); - float bearing = get_bearing_to_next_waypoint(curr_sp->lat, curr_sp->lon, next_sp->lat, next_sp->lon); + float bearing = get_bearing_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon); float inner_angle = M_PI_2_F - asinf(_mission_item.loiter_radius / range); // Compute "ideal" tangent origin - if (curr_sp->loiter_direction > 0) { + if (curr_sp.loiter_direction > 0) { bearing -= inner_angle; } else { bearing += inner_angle; } - // Replace current setpoint lat/lon with tangent coordinate - waypoint_from_heading_and_distance(curr_sp->lat, curr_sp->lon, - bearing, curr_sp->loiter_radius, - &curr_sp->lat, &curr_sp->lon); + waypoint_from_heading_and_distance(curr_sp.lat, curr_sp.lon, + bearing, curr_sp.loiter_radius, + &curr_sp.lat, &curr_sp.lon); } return true;