Plane: make TERRAIN_FOLLOW at AP_int32 bitmask, per mode option

ArduPlane/Parameters.cpp

@@ -310,6 +310,7 @@ const AP_Param::Info Plane::var_info[] = {
     // @DisplayName: Use terrain following
     // @Description: This enables terrain following for CRUISE mode, FBWB mode, RTL and for rally points. To use this option you also need to set TERRAIN_ENABLE to 1, which enables terrain data fetching from the GCS, and you need to have a GCS that supports sending terrain data to the aircraft. When terrain following is enabled then CRUISE and FBWB mode will hold height above terrain rather than height above home. In RTL the return to launch altitude will be considered to be a height above the terrain. Rally point altitudes will be taken as height above the terrain. This option does not affect mission items, which have a per-waypoint flag for whether they are height above home or height above the terrain. To use terrain following missions you need a ground station which can set the waypoint type to be a terrain height waypoint when creating the mission.
     // @Values: 0:Disabled,1:Enabled
+    // @Bitmask: 0: Enable all modes, 1:FBWB, 2:Cruise, 3:Auto, 4:RTL, 5:Avoid_ADSB, 6:Guided, 7:Loiter, 8:Circle, 9:QRTL, 10:QLand, 11:Qloiter
     // @User: Standard
     GSCALAR(terrain_follow, "TERRAIN_FOLLOW",  0),
 
@@ -1470,6 +1471,10 @@ void Plane::load_parameters(void)
         }
     }
 
+#if AP_TERRAIN_AVAILABLE
+    g.terrain_follow.convert_parameter_width(AP_PARAM_INT8);
+#endif
+
     hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
 }
 

ArduPlane/Parameters.h

@@ -470,7 +470,7 @@ public:
     AP_Float takeoff_pitch_limit_reduction_sec;
     AP_Int8 level_roll_limit;
 #if AP_TERRAIN_AVAILABLE
-    AP_Int8 terrain_follow;
+    AP_Int32 terrain_follow;
     AP_Int16 terrain_lookahead;
 #endif
     AP_Int16 glide_slope_min;

ArduPlane/Plane.h

@@ -788,6 +788,28 @@ private:
     // terrain disable for non AUTO modes, set with an RC Option switch
     bool non_auto_terrain_disable;
     bool terrain_disabled();
+#if AP_TERRAIN_AVAILABLE
+    bool terrain_enabled_in_current_mode() const;
+    enum class terrain_bitmask {
+        ALL             = 1U << 0,
+        FLY_BY_WIRE_B   = 1U << 1,
+        CRUISE          = 1U << 2,
+        AUTO            = 1U << 3,
+        RTL             = 1U << 4,
+        AVOID_ADSB      = 1U << 5,
+        GUIDED          = 1U << 6,
+        LOITER          = 1U << 7,
+        CIRCLE          = 1U << 8,
+        QRTL            = 1U << 9,
+        QLAND           = 1U << 10,
+        QLOITER         = 1U << 11,
+    };
+    struct TerrainLookupTable{
+       Mode::Number mode_num;
+       terrain_bitmask bitmask;
+    };
+    static const TerrainLookupTable Terrain_lookup[];
+#endif
 
     // Attitude.cpp
     void adjust_nav_pitch_throttle(void);

ArduPlane/altitude.cpp

@@ -200,7 +200,7 @@ void Plane::set_target_altitude_current(void)
 #if AP_TERRAIN_AVAILABLE
     // also record the terrain altitude if possible
     float terrain_altitude;
-    if (g.terrain_follow && terrain.height_above_terrain(terrain_altitude, true) && !terrain_disabled()) {
+    if (terrain_enabled_in_current_mode() && terrain.height_above_terrain(terrain_altitude, true) && !terrain_disabled()) {
         target_altitude.terrain_following = true;
         target_altitude.terrain_alt_cm = terrain_altitude*100;
     } else {
@@ -462,7 +462,7 @@ bool Plane::above_location_current(const Location &loc)
 void Plane::setup_terrain_target_alt(Location &loc) const
 {
 #if AP_TERRAIN_AVAILABLE
-    if (g.terrain_follow) {
+    if (terrain_enabled_in_current_mode()) {
         loc.terrain_alt = true;
     }
 #endif
@@ -618,7 +618,7 @@ void Plane::rangefinder_terrain_correction(float &height)
 #if AP_TERRAIN_AVAILABLE
     if (!g.rangefinder_landing ||
         flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND ||
-        g.terrain_follow == 0) {
+        !terrain_enabled_in_current_mode()) {
         return;
     }
     float terrain_amsl1, terrain_amsl2;
@@ -690,7 +690,7 @@ void Plane::rangefinder_height_update(void)
 #if AP_TERRAIN_AVAILABLE
         // if we are terrain following then correction is based on terrain data
         float terrain_altitude;
-        if ((target_altitude.terrain_following || g.terrain_follow) && 
+        if ((target_altitude.terrain_following || terrain_enabled_in_current_mode()) && 
             terrain.height_above_terrain(terrain_altitude, true)) {
             correction = terrain_altitude - rangefinder_state.height_estimate;
         }
@@ -730,3 +730,41 @@ bool Plane::terrain_disabled()
 }
 
 
+/*
+  Check if terrain following is enabled for the current mode
+ */
+#if AP_TERRAIN_AVAILABLE
+const Plane::TerrainLookupTable Plane::Terrain_lookup[] = {
+    {Mode::Number::FLY_BY_WIRE_B, terrain_bitmask::FLY_BY_WIRE_B},
+    {Mode::Number::CRUISE, terrain_bitmask::CRUISE},
+    {Mode::Number::AUTO, terrain_bitmask::AUTO},
+    {Mode::Number::RTL, terrain_bitmask::RTL},
+    {Mode::Number::AVOID_ADSB, terrain_bitmask::AVOID_ADSB},
+    {Mode::Number::GUIDED, terrain_bitmask::GUIDED},
+    {Mode::Number::LOITER, terrain_bitmask::LOITER},
+    {Mode::Number::CIRCLE, terrain_bitmask::CIRCLE},
+    {Mode::Number::QRTL, terrain_bitmask::QRTL},
+    {Mode::Number::QLAND, terrain_bitmask::QLAND},
+    {Mode::Number::QLOITER, terrain_bitmask::QLOITER},
+};
+
+bool Plane::terrain_enabled_in_current_mode() const
+{
+    // Global enable
+    if ((g.terrain_follow.get() & int32_t(terrain_bitmask::ALL)) != 0) {
+        return true;
+    }
+
+    // Specific enable
+    for (const struct TerrainLookupTable entry : Terrain_lookup) {
+        if (entry.mode_num == control_mode->mode_number()) {
+            if ((g.terrain_follow.get() & int32_t(entry.bitmask)) != 0) {
+                return true;
+            }
+            break;
+        }
+    }
+
+    return false;
+}
+#endif