diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 44e52762bf..81fc3dbff1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -679,6 +679,7 @@ Mission::set_mission_items() _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.autocontinue = true; _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; } /* transition to MC */ diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 46a0c0cec9..bb3caf2628 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -66,22 +66,23 @@ orb_advert_t actuator_pub_fd; MissionBlock::MissionBlock(Navigator *navigator, const char *name) : NavigatorMode(navigator, name), - _mission_item( {0}), - _waypoint_position_reached(false), - _waypoint_yaw_reached(false), - _time_first_inside_orbit(0), - _action_start(0), - _time_wp_reached(0), - _actuators{}, - _actuator_pub(nullptr), - _cmd_pub(nullptr), - _param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false), - _param_yaw_timeout(this, "MIS_YAW_TMT", false), - _param_yaw_err(this, "MIS_YAW_ERR", false), - _param_vtol_wv_land(this, "VT_WV_LND_EN", false), - _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) + _mission_item({0}), + _waypoint_position_reached(false), + _waypoint_yaw_reached(false), + _time_first_inside_orbit(0), + _action_start(0), + _time_wp_reached(0), + _actuators{}, + _actuator_pub(nullptr), + _cmd_pub(nullptr), + _param_loiter_min_alt(this, "MIS_LTRMIN_ALT", false), + _param_yaw_timeout(this, "MIS_YAW_TMT", false), + _param_yaw_err(this, "MIS_YAW_ERR", false), + _param_vtol_wv_land(this, "VT_WV_LND_EN", false), + _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) { } @@ -305,6 +306,19 @@ MissionBlock::is_mission_item_reached() mission_acceptance_radius = _navigator->get_acceptance_radius(); } + /* 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); + + 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 (dist >= 0.0f && dist <= mission_acceptance_radius && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 573d234819..4676cf3846 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -151,6 +151,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; }; #endif diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 46a5a5832f..54576f6506 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -124,13 +124,14 @@ struct mission_item_s { uint16_t do_jump_repeat_count; /**< how many times do jump needs to be done */ uint16_t do_jump_current_count; /**< count how many times the jump has been done */ struct { - uint16_t frame : 4, /**< mission frame ***/ - origin : 3, /**< how the mission item was generated */ - loiter_exit_xtrack : 1, /**< exit xtrack location: 0 for center of loiter wp, 1 for exit location */ - force_heading : 1, /**< heading needs to be reached ***/ - altitude_is_relative : 1, /**< true if altitude is relative from start point */ - autocontinue : 1, /**< true if next waypoint should follow after this one */ - disable_mc_yaw : 1; /**< weathervane mode */ + uint16_t frame : 4, /**< mission frame ***/ + origin : 3, /**< how the mission item was generated */ + loiter_exit_xtrack : 1, /**< exit xtrack location: 0 for center of loiter wp, 1 for exit location */ + force_heading : 1, /**< heading needs to be reached ***/ + altitude_is_relative : 1, /**< true if altitude is relative from start point */ + autocontinue : 1, /**< true if next waypoint should follow after this one */ + disable_mc_yaw : 1, /**< weathervane mode */ + vtol_back_transition : 1; /**< part of the vtol back transition sequence */ }; }; #pragma pack(pop)