diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index ef6dd9286e4088b33c1e3ff8f5b1c706d435f7cf..91a1e91fe0371af9b9198b4ec42b28afb0d607cb 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -2056,8 +2056,10 @@ static void update_altitude()
     baro_rate			= constrain(baro_rate, -500, 500);
 
     // read in sonar altitude and calculate sonar rate
-    if(g.sonar_enabled) {
-        sonar_alt       = read_sonar();
+    sonar_alt           = read_sonar();
+    // start calculating the sonar_rate as soon as valid sonar readings start coming in so that we are ready when the sonar_alt_health becomes 3
+    // Note: post 2.9.1 release we will remove the sonar_rate variable completely
+    if(sonar_alt_health > 1) {
         sonar_rate      = (sonar_alt - old_sonar_alt) * 10;
         sonar_rate      = constrain(sonar_rate, -150, 150);
     }
diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde
index 5f4b607e6a2f49d488dedfd9961e726fdba02260..0b5324badc2bdfe95a4293a98371d4bac3e1dce0 100644
--- a/ArduCopter/sensors.pde
+++ b/ArduCopter/sensors.pde
@@ -35,6 +35,12 @@ static int32_t read_barometer(void)
 static int16_t read_sonar(void)
 {
 #if CONFIG_SONAR == ENABLED
+    // exit immediately if sonar is disabled
+    if( !g.sonar_enabled ) {
+        sonar_alt_health = 0;
+        return 0;
+    }
+
     int16_t temp_alt = sonar.read();
 
     if(temp_alt >= sonar.min_distance && temp_alt <= sonar.max_distance * 0.70) {