diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-04 11:41:13 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-09-04 11:41:13 +0200 |
| commit | 67422c9896fb952ccf6db555aeb6f99224f8178b (patch) | |
| tree | 2c66825aaec0cfb76126c48dcc678018c4614ef6 /src/modules/navigator/mission.cpp | |
| parent | 9cf5e6f7b268acd732a14e577752dc32aeaf9430 (diff) | |
| download | px4-firmware-67422c9896fb952ccf6db555aeb6f99224f8178b.tar.gz px4-firmware-67422c9896fb952ccf6db555aeb6f99224f8178b.tar.bz2 px4-firmware-67422c9896fb952ccf6db555aeb6f99224f8178b.zip | |
foh alt mode: never sink below previous wp alt
Diffstat (limited to 'src/modules/navigator/mission.cpp')
| -rw-r--r-- | src/modules/navigator/mission.cpp | 6 |
1 files changed, 4 insertions, 2 deletions
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 7000dd6c0..b33b2049f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -502,8 +502,10 @@ Mission::altitude_sp_foh_update() float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); - /* Save distance to waypoint if it is the smallest ever achieved */ - _min_current_sp_distance_xy = math::min(d_current, _min_current_sp_distance_xy); + /* Save distance to waypoint if it is the smallest ever achieved, however make sure that + * _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp */ + _min_current_sp_distance_xy = math::min(math::min(d_current, _min_current_sp_distance_xy), + _distance_current_previous); /* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */ |