diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-01 09:49:54 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-01 09:49:54 +0200 |
commit | 98e7d2b998a2dec2436aafb8ee83c2b44b8c9ee4 (patch) | |
tree | 08fa0ab64578140138dea9636075370f396923f9 /src | |
parent | 90d8f7726d682e11039a313ebac6b047b59ccfb8 (diff) | |
parent | 95a8414895d0f93bf92b8339ad15a1b3b4d1a7f2 (diff) | |
download | px4-firmware-98e7d2b998a2dec2436aafb8ee83c2b44b8c9ee4.tar.gz px4-firmware-98e7d2b998a2dec2436aafb8ee83c2b44b8c9ee4.tar.bz2 px4-firmware-98e7d2b998a2dec2436aafb8ee83c2b44b8c9ee4.zip |
Merge pull request #998 from PX4/takeoff_fix
navigator: takeoff fix
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 32 |
1 files changed, 16 insertions, 16 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 401d50f7e..609356eb5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1477,27 +1477,27 @@ Navigator::check_mission_item_reached() acceptance_radius = _parameters.acceptance_radius; } - float dist = -1.0f; - float dist_xy = -1.0f; - float dist_z = -1.0f; - - /* calculate AMSL altitude for this waypoint */ - float wp_alt_amsl = _mission_item.altitude; - - if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.alt; - - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, - &dist_xy, &dist_z); - if (_do_takeoff) { - if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { - /* require only altitude for takeoff */ + /* require only altitude for takeoff */ + if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) { _waypoint_position_reached = true; } } else { + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + /* calculate AMSL altitude for this waypoint */ + float wp_alt_amsl = _mission_item.altitude; + + if (_mission_item.altitude_is_relative) + wp_alt_amsl += _home_pos.alt; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + &dist_xy, &dist_z); + if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; } |