Copter: send terrain alt to AHRS

Author: rmackay9

ArduCopter/terrain.cpp

@@ -6,6 +6,15 @@ void Copter::terrain_update()
 #if AP_TERRAIN_AVAILABLE
     terrain.update();
 
+    // send terrain altitude to AHRS for optical flow when rangefinder is out of range
+    Location veh_loc;
+    if (ahrs.get_location(veh_loc)) {
+        float terrain_alt_amsl_m;
+        if (terrain.height_amsl(veh_loc, terrain_alt_amsl_m, true)) {
+            ahrs.writeTerrainAMSL(terrain_alt_amsl_m);
+        }
+    }
+
     // tell the rangefinder our height, so it can go into power saving
     // mode if available
 #if AP_RANGEFINDER_ENABLED