diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-18 00:03:43 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-18 00:03:43 +0200 |
| commit | 1fea1a6015a0af829a440b2bba176488d6e6a0a3 (patch) | |
| tree | ca9c7f6a1f51957f83047192f15d3d2044f2c6c4 /src | |
| parent | e4bdf2f65a74407bfe6abb6f45c85c68e88cf6c8 (diff) | |
| parent | 101e7b1383b83c9b06ef147dbef1ff375c11b84f (diff) | |
| download | px4-firmware-1fea1a6015a0af829a440b2bba176488d6e6a0a3.tar.gz px4-firmware-1fea1a6015a0af829a440b2bba176488d6e6a0a3.tar.bz2 px4-firmware-1fea1a6015a0af829a440b2bba176488d6e6a0a3.zip | |
Merge pull request #1198 from PX4/wpwarningfix
Check if waypoint altitude is relative. Fixes #1197
Diffstat (limited to 'src')
| -rw-r--r-- | src/modules/navigator/mission_feasibility_checker.cpp | 9 |
1 files changed, 6 insertions, 3 deletions
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index dd7f4c801..606521f20 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -135,12 +135,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } - if (home_alt > missionitem.altitude) { + /* calculate the global waypoint altitude */ + float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; + + if (home_alt > wp_alt) { if (throw_error) { - mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); return false; } else { - mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); + mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i); return true; } } |