Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-04 05:05:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

mission_block: fix vehicle not exiting loiter after reaching exit heading

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2022-04-13 16:08:31 +03:00
committed by Silvan Fuhrer
parent d5a6174e7f
commit a7683eea07
1 changed files with 7 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+1 -9
View File
@@ -430,7 +430,6 @@ MissionBlock::is_mission_item_reached()
float yaw_err = 0.0f; float yaw_err = 0.0f;
if (dist_current_next > 1.2f * _navigator->get_loiter_radius()) {
// set required yaw from bearing to the next mission item // set required yaw from bearing to the next mission item
_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon, _navigator->get_global_position()->lon,
@@ -438,14 +437,7 @@ MissionBlock::is_mission_item_reached()
const float cog = atan2f(_navigator->get_local_position()->vy, _navigator->get_local_position()->vx); const float cog = atan2f(_navigator->get_local_position()->vy, _navigator->get_local_position()->vx);
yaw_err = wrap_pi(_mission_item.yaw - cog); yaw_err = wrap_pi(_mission_item.yaw - cog);
exit_heading_reached = fabsf(yaw_err) < _navigator->get_yaw_threshold();
}
if (fabsf(yaw_err) < _navigator->get_yaw_threshold()) {
exit_heading_reached = true;
}
} else { } else {
exit_heading_reached = true; exit_heading_reached = true;