From 4c674b64fbb514789cf0e30939692edb4f08138d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 4 Aug 2022 10:36:02 +1000 Subject: [PATCH] AP_Terrain: correct wait-for-terrain prearm check looking at pending is insufficient as we may have more mission items to check to see if they need other pieces of terrain. That is, terr_pending can go to zero momentarily and then go back to a non-zero number as Terrain's update method checks the mission and rally libraries for more terrain requirements. Without this patch the prearm checks can momentarily pass, allowing the vehicle to arm. The vehicle could hit a terrain failsafe later if it doesn't manage to get the data while in flight. --- libraries/AP_Terrain/AP_Terrain.cpp | 7 ++++++- libraries/AP_Terrain/AP_Terrain.h | 1 + 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Terrain/AP_Terrain.cpp b/libraries/AP_Terrain/AP_Terrain.cpp index ebd293664c..89d96ce047 100644 --- a/libraries/AP_Terrain/AP_Terrain.cpp +++ b/libraries/AP_Terrain/AP_Terrain.cpp @@ -166,6 +166,7 @@ bool AP_Terrain::height_amsl(const Location &loc, float &height, bool corrected) // remember home altitude as a special case home_height = height; home_loc = loc; + have_home_height = true; } if (corrected && have_reference_offset) { @@ -369,7 +370,11 @@ bool AP_Terrain::pre_arm_checks(char *failure_msg, uint8_t failure_msg_len) cons // check no outstanding requests for data: uint16_t terr_pending, terr_loaded; get_statistics(terr_pending, terr_loaded); - if (terr_pending != 0) { + if (terr_pending != 0 || + !have_current_loc_height || + !have_home_height || + next_mission_index != 0 || + next_rally_index != 0) { hal.util->snprintf(failure_msg, failure_msg_len, "waiting for terrain data"); return false; } diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h index f013081d46..7ae415d103 100644 --- a/libraries/AP_Terrain/AP_Terrain.h +++ b/libraries/AP_Terrain/AP_Terrain.h @@ -409,6 +409,7 @@ private: // cache the home altitude, as it is needed so often float home_height; Location home_loc; + bool have_home_height; // reference position for terrain adjustment, set at arming bool have_reference_loc;