diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 861aed8139..08576750c9 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -102,13 +102,19 @@ MissionBlock::is_mission_item_reached() _navigator_priv->get_global_position()->alt, &dist_xy, &dist_z); - if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - - /* require only altitude for takeoff */ - if (_navigator_priv->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) { + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) { + /* require only altitude for takeoff for multicopter */ + if (_navigator_priv->get_global_position()->alt > + altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) { + _waypoint_position_reached = true; + } + } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + /* for takeoff mission items use the parameter for the takeoff acceptance radius */ + if (dist >= 0.0f && dist <= _navigator_priv->get_takeoff_acceptance_radius()) { _waypoint_position_reached = true; } } else { + /* for normal mission items used their acceptance radius */ if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { _waypoint_position_reached = true; } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 296294a918..fe7485f566 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -109,6 +109,7 @@ public: Geofence& get_geofence() { return _geofence; } bool get_is_in_loiter() { return _is_in_loiter; } float get_loiter_radius() { return _param_loiter_radius.get(); } + float get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); } int get_mavlink_fd() { return _mavlink_fd; } private: @@ -156,6 +157,7 @@ private: bool _update_triplet; /**< flags if position SP triplet needs to be published */ control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ + control::BlockParamFloat _param_takeoff_acceptance_radius; /**< acceptance for takeoff */ /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index da06f826a6..dc85738134 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -122,7 +122,8 @@ Navigator::Navigator() : _loiter(this, "LOI"), _rtl(this, "RTL"), _update_triplet(false), - _param_loiter_radius(this, "LOITER_RAD") + _param_loiter_radius(this, "LOITER_RAD"), + _param_takeoff_acceptance_radius(this, "TF_ACC_RAD") { updateParams(); } diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 9235f3046b..ca31adecc6 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -53,3 +53,14 @@ * @group Mission */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); + +/** + * Takeoff Acceptance Radius (FW only) + * + * Acceptance radius for fixedwing. + * + * @unit meters + * @min 1.0 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);