From 590f00f56089cf6afa62b7f4df07527c014da559 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Dec 2015 10:50:38 +0100 Subject: [PATCH] Navigator: Set sane defaults for navigation and loiter waypoints with zero acceptance radius or orbit --- src/modules/navigator/mission_block.cpp | 5 +++-- src/modules/navigator/navigation.h | 2 ++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 80d8b3df3d..e2f454d4cf 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -146,7 +146,7 @@ MissionBlock::is_mission_item_reached() float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius); /* if set to zero use the default instead */ - if (mission_acceptance_radius < 0.001f) { + if (mission_acceptance_radius < NAV_EPSILON_POSITION) { mission_acceptance_radius = _navigator->get_acceptance_radius(); } @@ -210,7 +210,8 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->lon = item->lon; sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; sp->yaw = item->yaw; - sp->loiter_radius = item->loiter_radius; + sp->loiter_radius = (item->loiter_radius > NAV_EPSILON_POSITION) ? item->loiter_radius : + _navigator->get_loiter_radius(); sp->loiter_direction = item->loiter_direction; sp->pitch_min = item->pitch_min; diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index e6b8a68281..b0fabc08f5 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -47,6 +47,8 @@ #define NUM_MISSIONS_SUPPORTED 256 +#define NAV_EPSILON_POSITION 0.001f /**< Anything smaller than this is considered zero */ + /* compatible to mavlink MAV_CMD */ enum NAV_CMD { NAV_CMD_IDLE = 0,