diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 23b26ec42c..0866ab3d2d 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -798,6 +798,7 @@ private: bool terrain_disabled(); #if AP_TERRAIN_AVAILABLE bool terrain_enabled_in_current_mode() const; + bool terrain_enabled_in_mode(Mode::Number num) const; enum class terrain_bitmask { ALL = 1U << 0, FLY_BY_WIRE_B = 1U << 1, diff --git a/ArduPlane/altitude.cpp b/ArduPlane/altitude.cpp index 6b2e9f0379..6fec0063e8 100644 --- a/ArduPlane/altitude.cpp +++ b/ArduPlane/altitude.cpp @@ -765,6 +765,11 @@ const Plane::TerrainLookupTable Plane::Terrain_lookup[] = { }; bool Plane::terrain_enabled_in_current_mode() const +{ + return terrain_enabled_in_mode(control_mode->mode_number()); +} + +bool Plane::terrain_enabled_in_mode(Mode::Number num) const { // Global enable if ((g.terrain_follow.get() & int32_t(terrain_bitmask::ALL)) != 0) { @@ -773,7 +778,7 @@ bool Plane::terrain_enabled_in_current_mode() const // Specific enable for (const struct TerrainLookupTable entry : Terrain_lookup) { - if (entry.mode_num == control_mode->mode_number()) { + if (entry.mode_num == num) { if ((g.terrain_follow.get() & int32_t(entry.bitmask)) != 0) { return true; }