diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 24dc38437f..9b46e78e13 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -65,7 +65,8 @@ MissionBlock::MissionBlock(Navigator *navigator, const char *name) : _param_vtol_wv_takeoff(this, "VT_WV_TKO_EN", false), _param_vtol_wv_loiter(this, "VT_WV_LTR_EN", false), _param_force_vtol(this, "NAV_FORCE_VT", false), - _param_back_trans_dec_mss(this, "VT_B_DEC_MSS", false) + _param_back_trans_dec_mss(this, "VT_B_DEC_MSS", false), + _param_reverse_delay(this, "VT_B_REV_DEL", false) { } @@ -300,7 +301,9 @@ MissionBlock::is_mission_item_reached() _navigator->get_local_position()->vy * _navigator->get_local_position()->vy); if (_param_back_trans_dec_mss.get() > FLT_EPSILON && velocity > FLT_EPSILON) { - mission_acceptance_radius = (velocity / _param_back_trans_dec_mss.get() / 2) * velocity; + mission_acceptance_radius = ((velocity / _param_back_trans_dec_mss.get() / 2) * velocity) + _param_reverse_delay.get() * + velocity; + } } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 4cee8ddcb9..41dce6837f 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -149,6 +149,7 @@ protected: control::BlockParamInt _param_vtol_wv_loiter; control::BlockParamInt _param_force_vtol; control::BlockParamFloat _param_back_trans_dec_mss; + control::BlockParamFloat _param_reverse_delay; }; #endif