diff options
author | Julian Oes <julian@oes.ch> | 2014-06-10 17:24:41 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-06-10 17:24:41 +0200 |
commit | 0697bb29a2aaf2f14e1a7477008b118a0696e54e (patch) | |
tree | ff04688eeb509bcf22bd680b3d5a7689b36fffef /src/modules | |
parent | f8b4295cfdca946b24acb210af22bf15d7c04ab4 (diff) | |
parent | fd1f1c81efb384e46a5767555a58061082612c9f (diff) | |
download | px4-firmware-0697bb29a2aaf2f14e1a7477008b118a0696e54e.tar.gz px4-firmware-0697bb29a2aaf2f14e1a7477008b118a0696e54e.tar.bz2 px4-firmware-0697bb29a2aaf2f14e1a7477008b118a0696e54e.zip |
Merge branch 'navigator_rewrite' into navigator_rewrite_estimator
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/navigator/mission_block.cpp | 14 | ||||
-rw-r--r-- | src/modules/navigator/navigator.h | 2 | ||||
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 3 | ||||
-rw-r--r-- | src/modules/navigator/navigator_params.c | 11 |
4 files changed, 25 insertions, 5 deletions
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 861aed813..08576750c 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 296294a91..fe7485f56 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 da06f826a..dc8573813 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 9235f3046..ca31adecc 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); |