From b05465470a41e198db69e9978eb70b781d7def18 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 2 Feb 2016 23:04:56 +0100 Subject: [PATCH] ignore wp altitude for intermediate landing wp so it doesn't descent before it is in landing mode --- src/modules/navigator/mission.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 2de3d23289..c50149597e 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -458,9 +458,20 @@ Mission::set_mission_items() memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s)); has_next_position_item = true; + /* + * Ignoring waypoint altitude: + * Set altitude to the same as we have now to prevent descending too fast into + * the ground. Actual landing will descend anyway until it touches down. + */ + float altitude = _navigator->get_global_position()->alt; + if (pos_sp_triplet->current.valid) { + altitude = pos_sp_triplet->current.alt; + } + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.autocontinue = true; _mission_item.time_inside = 0; + _mission_item.altitude = altitude; } /* we just moved to the landing waypoint, now descend */