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

transition alignment will force heading now and go to RTL if it cannot reach it in time, handle mission failure correctly, reset after mission update, issue message with actual problem

Browse Source
This commit is contained in:
Andreas Antener
2016-02-15 23:29:38 +01:00
parent bb4decfa8b
commit eb5b8a32ee
11 changed files with 46 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission_block.cpp
+9 -1
View File
@@ -196,9 +196,17 @@ MissionBlock::is_mission_item_reached()
float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
if (fabsf(yaw_err) < math::radians(_param_yaw_err.get())
/* check if we should accept heading after a timeout, 0 means accept instantly */
|| (_param_yaw_timeout.get() >= -FLT_EPSILON &&
now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) {
_waypoint_yaw_reached = true;
/* if heading needs to be reached but we got here because of the timeout, abort mission */
if (_mission_item.force_heading && !(fabsf(yaw_err) < math::radians(_param_yaw_err.get()))) {
_navigator->set_mission_failure("unable to reach heading within timeout");
} else {
_waypoint_yaw_reached = true;
}
}
} else {