dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Navigator: mission_block: reduce enforce eexit course margin to 105% (#22511) 

With this margin it is made sure that if the loiter is not perfectly tracked,
(vehicle outside of path setpoint) a wp just at the border of the loiter
is still reachable.
It should though be as small as necessary, as otherwise, with good
loiter tracking,  waypoints that are close but not right on the loiter
radius are not enforcing the exit course neither.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-12-21 16:58:22 +01:00 committed by GitHub
parent 7e22b47b85
commit 6be8cbe439
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/navigator/mission_block.cpp
View File

@ -465,8 +465,8 @@ MissionBlock::is_mission_item_reached_or_completed()
&& 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);
// can only enforce exit course if next waypoint is not within loiter radius of current waypoint // can only enforce exit course if next waypoint is not within loiter radius of current waypoint (with small margin)
const bool exit_course_is_reachable = dist_current_next > 1.2f * curr_sp_new->loiter_radius; const bool exit_course_is_reachable = dist_current_next > 1.05f * curr_sp_new->loiter_radius;
if (enforce_exit_course && exit_course_is_reachable) { if (enforce_exit_course && exit_course_is_reachable) {