From bad0120d3c8e6ff2d56774002072386cc7ee3df8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 23 Jun 2020 07:23:46 +1000 Subject: [PATCH] AP_Mission: added contains_terrain_relative() used in plane arming check --- libraries/AP_Mission/AP_Mission.cpp | 17 +++++++++++++++++ libraries/AP_Mission/AP_Mission.h | 3 +++ 2 files changed, 20 insertions(+) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 5affe83f3c..ce3991ae67 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -2464,6 +2464,23 @@ bool AP_Mission::contains_item(MAV_CMD command) const return false; } +/* + return true if the mission has a terrain relative item + */ +bool AP_Mission::contains_terrain_relative(void) const +{ + for (int i = 1; i < num_commands(); i++) { + Mission_Command tmp; + if (!read_cmd_from_storage(i, tmp)) { + continue; + } + if (stored_in_location(tmp.id) && tmp.content.location.terrain_alt) { + return true; + } + } + return false; +} + // reset the mission history to prevent recalling previous mission histories after a mission restart. void AP_Mission::reset_wp_history(void) { diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 8b21e603a5..a35a6b2fe8 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -619,6 +619,9 @@ public: // returns true if the mission contains the requested items bool contains_item(MAV_CMD command) const; + // returns true if the mission has a terrain relative mission item + bool contains_terrain_relative(void) const; + // reset the mission history to prevent recalling previous mission histories when restarting missions. void reset_wp_history(void);