Plane: added TERRAIN_LOOKAHD parameter

distance to look forward for terrain following

ArduPlane/ArduPlane.pde

@@ -688,6 +688,9 @@ static struct {
     // target altitude above terrain in cm, valid if terrain_following
     // is set
     int32_t terrain_alt_cm;
+
+    // lookahead value for height error reporting
+    float lookahead;
 #endif
 } target_altitude;
 

ArduPlane/Parameters.h

@@ -121,6 +121,7 @@ public:
         k_param_stab_pitch_down_cd_old, // deprecated
         k_param_glide_slope_threshold,
         k_param_stab_pitch_down,
+        k_param_terrain_lookahead,
 
         // 100: Arming parameters
         k_param_arming = 100,
@@ -453,6 +454,7 @@ public:
     AP_Int8 flaperon_output;
 #if AP_TERRAIN_AVAILABLE
     AP_Int8 terrain_follow;
+    AP_Int16 terrain_lookahead;
 #endif
     AP_Int16 glide_slope_threshold;
 

ArduPlane/Parameters.pde

@@ -391,6 +391,14 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Values: 0:Disabled,1:Enabled
     // @User: Standard
     GSCALAR(terrain_follow, "TERRAIN_FOLLOW",  0),
+
+    // @Param: TERRAIN_LOOKAHD
+    // @DisplayName: Terrain lookahead
+    // @Description: This controls how far ahead the terrain following code looks to ensure it stays above upcoming terrain. A value of zero means no lookahead, so the controller will track only the terrain directly below the aircraft. The lookahead will never extend beyond the next waypoint when in AUTO mode.
+    // @Range: 0 10000
+    // @Units: meters
+    // @User: Standard
+    GSCALAR(terrain_lookahead, "TERRAIN_LOOKAHD",  2000),
 #endif
 
     // @Param: FBWB_CLIMB_RATE

ArduPlane/altitude.pde

@@ -200,6 +200,9 @@ static int32_t relative_target_altitude_cm(void)
     if (target_altitude.terrain_following && 
         terrain.height_relative_home_equivalent(target_altitude.terrain_alt_cm*0.01f,
                                                 relative_home_height, true)) {
+        // add lookahead adjustment the target altitude
+        target_altitude.lookahead = lookahead_adjustment();
+        relative_home_height += target_altitude.lookahead;
         // we are following terrain, and have terrain data for the
         // current location. Use it.
         return relative_home_height*100;
@@ -262,7 +265,7 @@ static int32_t calc_altitude_error_cm(void)
     float terrain_height;
     if (target_altitude.terrain_following && 
         terrain.height_above_terrain(terrain_height, true)) {
-        return target_altitude.terrain_alt_cm - (terrain_height*100);
+        return target_altitude.lookahead*100 + target_altitude.terrain_alt_cm - (terrain_height*100);
     }
 #endif
     return target_altitude.amsl_cm - adjusted_altitude_cm();
@@ -394,3 +397,59 @@ static int32_t adjusted_altitude_cm(void)
 {
     return current_loc.alt - (g.alt_offset*100);
 }
+
+/*
+  work out target altitude adjustment from terrain lookahead
+ */
+static float lookahead_adjustment(void)
+{
+#if AP_TERRAIN_AVAILABLE
+    int32_t bearing_cd;
+    int16_t distance;
+    // work out distance and bearing to target
+    if (control_mode == FLY_BY_WIRE_B) {
+        // there is no target waypoint in FBWB, so use yaw as an approximation
+        bearing_cd = ahrs.yaw_sensor;
+        distance = g.terrain_lookahead;
+    } else if (!nav_controller->reached_loiter_target()) {
+        bearing_cd = nav_controller->target_bearing_cd();
+        distance = constrain_float(wp_distance, 0, g.terrain_lookahead);
+    } else {
+        // no lookahead when loitering
+        bearing_cd = 0;
+        distance = 0;
+    }
+    if (distance <= 0) {
+        // no lookahead
+        return 0;
+    }
+
+    
+    float groundspeed = ahrs.groundspeed();
+    if (groundspeed < 1) {
+        // we're not moving
+        return 0;
+    }
+    // we need to know the climb ratio. We use 50% of the maximum
+    // climb rate so we are not constantly at 100% throttle and to
+    // give a bit more margin on terrain
+    float climb_ratio = 0.5f * SpdHgt_Controller->get_max_climbrate() / groundspeed;
+
+    if (climb_ratio <= 0) {
+        // lookahead makes no sense for negative climb rates
+        return 0;
+    }
+    
+    // ask the terrain code for the lookahead altitude change
+    float lookahead = terrain.lookahead(bearing_cd*0.01f, distance, climb_ratio);
+    
+    if (target_altitude.offset_cm < 0) {
+        // we are heading down to the waypoint, so we don't need to
+        // climb as much
+        lookahead += target_altitude.offset_cm*0.01f;
+    }
+
+    // constrain lookahead to a reasonable limit
+    return constrain_float(lookahead, 0, 1000.0f);
+#endif
+}