From e9516db5b0c9fb1e6dff51ede427d4cfd2d7bf3c Mon Sep 17 00:00:00 2001 From: sanderux Date: Thu, 27 Jul 2017 01:54:41 +0200 Subject: [PATCH] Back transition duration based on deceleration m/s/s --- src/modules/navigator/mission_block.cpp | 11 +++++------ src/modules/navigator/mission_block.h | 2 +- .../vtol_att_control/vtol_att_control_params.c | 14 ++++++++++++++ 3 files changed, 20 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 35a7e896c9..24dc38437f 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -65,7 +65,7 @@ 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_dur(this, "VT_B_TRANS_DUR", false) + _param_back_trans_dec_mss(this, "VT_B_DEC_MSS", false) { } @@ -296,12 +296,11 @@ MissionBlock::is_mission_item_reached() /* for vtol back transition calculate acceptance radius based on time and ground speed */ if (_mission_item.vtol_back_transition) { - float groundspeed = sqrtf(_navigator->get_global_position()->vel_n * _navigator->get_global_position()->vel_n + - _navigator->get_global_position()->vel_e * _navigator->get_global_position()->vel_e); + float velocity = sqrtf(_navigator->get_local_position()->vx * _navigator->get_local_position()->vx + + _navigator->get_local_position()->vy * _navigator->get_local_position()->vy); - if (_param_back_trans_dur.get() > FLT_EPSILON && groundspeed > FLT_EPSILON - && groundspeed * _param_back_trans_dur.get() > mission_acceptance_radius) { - mission_acceptance_radius = groundspeed * _param_back_trans_dur.get(); + if (_param_back_trans_dec_mss.get() > FLT_EPSILON && velocity > FLT_EPSILON) { + mission_acceptance_radius = (velocity / _param_back_trans_dec_mss.get() / 2) * velocity; } } diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index b14d00c77f..4cee8ddcb9 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -148,7 +148,7 @@ protected: control::BlockParamInt _param_vtol_wv_takeoff; control::BlockParamInt _param_vtol_wv_loiter; control::BlockParamInt _param_force_vtol; - control::BlockParamFloat _param_back_trans_dur; + control::BlockParamFloat _param_back_trans_dec_mss; }; #endif diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index a8bca84e51..1ce7adeb0a 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -220,6 +220,20 @@ PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 5.0f); */ PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 4.0f); +/** + * Deceleration during back transition + * + * The deceleration during a back transition in m/s/s + * + * @unit m/s/s + * @min 0.00 + * @max 20.00 + * @increment 1 + * @decimal 2 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_DEC_MSS, 2.0f); + /** * Output on reverse channel during back transition *