SITL: implement SIM_TERRAIN

either use AP_Terrain or flat earth

libraries/AP_HAL_AVR_SITL/SITL_State.cpp

@@ -669,4 +669,34 @@ void SITL_State::loop_hook(void)
 }
 
 
+/*
+  return height above the ground in meters
+ */
+float SITL_State::height_agl(void)
+{
+	static float home_alt = -1;
+
+	if (home_alt == -1 && _sitl->state.altitude > 0) {
+        // remember home altitude as first non-zero altitude
+		home_alt = _sitl->state.altitude;
+    }
+
+    if (_terrain &&
+        _sitl->terrain_enable) {
+        // get height above terrain from AP_Terrain. This assumes
+        // AP_Terrain is working
+        float terrain_height_amsl;
+        struct Location location;
+        location.lat = _sitl->state.latitude*1.0e7;
+        location.lng = _sitl->state.longitude*1.0e7;
+
+        if (_terrain->height_amsl(location, terrain_height_amsl)) {
+            return _sitl->state.altitude - terrain_height_amsl;
+        }
+    }
+
+    // fall back to flat earth model
+    return _sitl->state.altitude - home_alt;
+}
+
 #endif

libraries/AP_HAL_AVR_SITL/SITL_State.h

@@ -63,6 +63,7 @@ private:
 
     // these methods are static as they are called
     // from the timer
+    static float height_agl(void);
     static void _update_barometer(float height);
     static void _update_compass(float rollDeg, float pitchDeg, float yawDeg);
     static void _update_flow(void);
@@ -105,7 +106,7 @@ private:
     static void _simulator_output(void);
     static void _apply_servo_filter(float deltat);
     static uint16_t _airspeed_sensor(float airspeed);
-    static uint16_t _ground_sonar(float altitude);
+    static uint16_t _ground_sonar();
     static float _gyro_drift(void);
     static float _rand_float(void);
     static Vector3f _rand_vec3f(void);

libraries/AP_HAL_AVR_SITL/sitl_ins.cpp

@@ -60,33 +60,31 @@ float SITL_State::_gyro_drift(void)
 
 }
 
-uint16_t SITL_State::_ground_sonar(float altitude)
+/*
+  emulate an analog rangefinder
+ */
+uint16_t SITL_State::_ground_sonar(void)
 {
-	static float home_alt = -1;
+    float altitude = height_agl();
-
+
-	if (home_alt == -1 && altitude > 0)
+    float voltage = 5.0f;
-		home_alt = altitude;
+    if (fabsf(_sitl->state.rollDeg) < 90 &&
-
+        fabsf(_sitl->state.pitchDeg) < 90) {
-	altitude = altitude - home_alt;
+        // adjust for apparent altitude with roll
-
+        altitude /= cos(radians(_sitl->state.rollDeg)) * cos(radians(_sitl->state.pitchDeg));
-        float voltage = 5.0f;
+        
-        if (fabsf(_sitl->state.rollDeg) < 90 &&
+        altitude += _sitl->sonar_noise * _rand_float();
-            fabsf(_sitl->state.pitchDeg) < 90) {
+
-            // adjust for apparent altitude with roll
+        // Altitude in in m, scaler in meters/volt
-            altitude /= cos(radians(_sitl->state.rollDeg)) * cos(radians(_sitl->state.pitchDeg));
+        voltage = altitude / _sitl->sonar_scale;
-
+        voltage = constrain_float(voltage, 0, 5.0f);
-            altitude += _sitl->sonar_noise * _rand_float();
+        
-
+        if (_sitl->sonar_glitch >= (_rand_float() + 1.0f)/2.0f) {
-            // Altitude in in m, scaler in meters/volt
+            voltage = 5.0f;
-            voltage = altitude / _sitl->sonar_scale;
-            voltage = constrain_float(voltage, 0, 5.0f);
-
-            if (_sitl->sonar_glitch >= (_rand_float() + 1.0f)/2.0f) {
-                voltage = 5.0f;
-            }
         }
-
+    }
-        return 1023*(voltage / 5.0f);
+    
+    return 1023*(voltage / 5.0f);
 }
 
 /*
@@ -177,7 +175,7 @@ void SITL_State::_update_ins(float roll, 	float pitch, 	float yaw,		// Relative
 	_ins->set_gyro(1, Vector3f(p2, q2, r2) + _ins->get_gyro_offsets(1));
 
 
-        sonar_pin_value    = _ground_sonar(altitude);
+        sonar_pin_value    = _ground_sonar();
         airspeed_pin_value = _airspeed_sensor(airspeed + (_sitl->aspd_noise * _rand_float()));
 }
 

libraries/AP_HAL_AVR_SITL/sitl_optical_flow.cpp

@@ -32,7 +32,6 @@ void SITL_State::_update_flow(void)
     Vector3f gyro;
 
     if (!_optical_flow ||
-        !_terrain ||
         !_sitl->flow_enable) {
         return;
     }
@@ -48,20 +47,6 @@ void SITL_State::_update_flow(void)
 
     OpticalFlow::OpticalFlow_state state;
 
-    // get height above terrain from AP_Terrain. This assumes
-    // AP_Terrain is working
-    float terrain_height_amsl;
-    struct Location location;
-    location.lat = _sitl->state.latitude*1.0e7;
-    location.lng = _sitl->state.longitude*1.0e7;
-
-    if (!_terrain->height_amsl(location, terrain_height_amsl)) {
-        // no terrain height available
-        return;
-    }
-
-    float height_agl = _sitl->state.altitude - terrain_height_amsl;
-
     // NED velocity vector in m/s
     Vector3f velocity(_sitl->state.speedN, 
                       _sitl->state.speedE, 
@@ -80,7 +65,7 @@ void SITL_State::_update_flow(void)
     // estimate range to centre of image
     float range;
     if (rotmat.c.z > 0.05f) {
-        range = height_agl / rotmat.c.z;
+        range = height_agl() / rotmat.c.z;
     } else {
         range = 1e38f;
     }