From 1fb743412810727d18f2815f541be1f75c11bfeb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Jun 2015 19:20:42 +0200 Subject: [PATCH] Navigation capabilites: Ensure regular publication of updated topic --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 ++- src/modules/navigator/navigator_main.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index b882cc08c4..350706f263 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1676,7 +1676,8 @@ FixedwingPositionControl::task_main() float turn_distance = _l1_control.switch_distance(100.0f); /* lazily publish navigation capabilities */ - if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) { + if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON + && turn_distance > 0)) { /* set new turn distance */ _nav_capabilities.turn_distance = turn_distance; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e05d079a0a..97baa1235f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -582,7 +582,7 @@ Navigator::get_acceptance_radius(float mission_item_radius) { float radius = mission_item_radius; - if (hrt_elapsed_time(&_nav_caps.timestamp) < 1000000) { + if (hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) { if (_nav_caps.turn_distance > radius) { radius = _nav_caps.turn_distance; }