fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 15:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mission_block: don't require an exiting heading when loitering if the next

Browse Source 
waypoint is within the loiter radius of the current waypoint

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2021-10-27 11:01:13 +03:00
committed by Roman Bapst
parent 5e1f62e9d0
commit d678e792cc
1 changed files with 6 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+6 -4
View File
@@ -375,6 +375,9 @@ MissionBlock::is_mission_item_reached()
struct position_setpoint_s *curr_sp_new = &_navigator->get_position_setpoint_triplet()->current; struct position_setpoint_s *curr_sp_new = &_navigator->get_position_setpoint_triplet()->current;
const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next; const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat,
next_sp.lon);
/* enforce exit heading if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set, /* enforce exit heading if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set,
or if loitering to achieve altitdue at a NAV_CMD_WAYPOINT */ or if loitering to achieve altitdue at a NAV_CMD_WAYPOINT */
const bool enforce_exit_heading = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING const bool enforce_exit_heading = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
@@ -383,11 +386,10 @@ MissionBlock::is_mission_item_reached()
curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER && curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER &&
(_mission_item.force_heading || _mission_item.nav_cmd == NAV_CMD_WAYPOINT); (_mission_item.force_heading || _mission_item.nav_cmd == NAV_CMD_WAYPOINT);
if (enforce_exit_heading) { // can only enforce exit heading if next waypoint is not within loiter radius of current waypoint
const bool exit_heading_is_reachable = dist_current_next > 1.2f * curr_sp_new->loiter_radius;
if (enforce_exit_heading && exit_heading_is_reachable) {
const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat,
next_sp.lon);
float yaw_err = 0.0f; float yaw_err = 0.0f;