yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

FW implement MAV_CMD_DO_GO_AROUND

Browse Source
This commit is contained in:
Daniel Agar
2016-07-29 20:24:21 -04:00
committed by Lorenz Meier
parent aa3ffa28c8
commit 2a15578f8d
9 changed files with 186 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -276,14 +276,16 @@ MissionBlock::is_mission_item_reached()
/* accept yaw if reached or if timeout is set in which case we ignore not forced headings */
if (fabsf(yaw_err) < math::radians(_param_yaw_err.get())
|| (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) {
|| (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) {
_waypoint_yaw_reached = true;
}
/* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */
if (!_waypoint_yaw_reached && _mission_item.force_heading &&
_param_yaw_timeout.get() >= FLT_EPSILON &&
now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f) {
(_param_yaw_timeout.get() >= FLT_EPSILON) &&
(now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) {
_navigator->set_mission_failure("unable to reach heading within timeout");
}
@@ -403,6 +405,7 @@ MissionBlock::item_contains_position(const struct mission_item_s *item)
item->nav_cmd == NAV_CMD_DO_MOUNT_CONTROL ||
item->nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST ||
item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) {
return false;
}