fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-15 10:27:34 +08:00
Code Issues 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
+7 -15
View File
@@ -430,22 +430,14 @@ MissionBlock::is_mission_item_reached()
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
_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
next_sp.lat, next_sp.lon);
const float cog = atan2f(_navigator->get_local_position()->vy, _navigator->get_local_position()->vx);
yaw_err = wrap_pi(_mission_item.yaw - cog);
// set required yaw from bearing to the next mission item
_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
next_sp.lat, next_sp.lon);
const float cog = atan2f(_navigator->get_local_position()->vy, _navigator->get_local_position()->vx);
yaw_err = wrap_pi(_mission_item.yaw - cog);
}
if (fabsf(yaw_err) < _navigator->get_yaw_threshold()) {
exit_heading_reached = true;
}
exit_heading_reached = fabsf(yaw_err) < _navigator->get_yaw_threshold();
} else {
exit_heading_reached = true;