diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
index 71875995a7..7dceb3dc26 100644
--- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
+++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp
@@ -69,17 +69,21 @@ uint16_t SITL_State::_ground_sonar(float altitude)
 
 	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 &&
+            fabsf(_sitl->state.pitchDeg) < 90) {
+            // adjust for apparent altitude with roll
+            altitude /= cos(radians(_sitl->state.rollDeg)) * cos(radians(_sitl->state.pitchDeg));
 
-	altitude += _sitl->sonar_noise * _rand_float();
+            altitude += _sitl->sonar_noise * _rand_float();
 
-	// Altitude in in m, scaler in meters/volt
-	float voltage = altitude / _sitl->sonar_scale;
-        voltage = constrain_float(voltage, 0, 5.0f);
+            // Altitude in in m, scaler in meters/volt
+            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;
+            if (_sitl->sonar_glitch >= (_rand_float() + 1.0f)/2.0f) {
+                voltage = 5.0f;
+            }
         }
 
         return 1023*(voltage / 5.0f);