Copter: fixed rangefinder filter when out of range

don't input invalid data into the filter, allowing it to reset if data
is more than 1s old

ArduCopter/sensors.cpp

@@ -72,20 +72,18 @@ void Copter::read_rangefinder(void)
         target_filter_hz = RANGEFINDER_WPNAV_FILT_MIN_HZ;
     }
 
-    // prevent sharp changes to the filter time constant
-    const float alt_filter_slew_alpha = 0.98;
-    rangefinder_state.filter_hz = rangefinder_state.filter_hz * alt_filter_slew_alpha + target_filter_hz * (1-alt_filter_slew_alpha);
-
-    if (target_filter_hz > rangefinder_state.filter_hz) {
-        // when raising target freq we use a reset to prevent a numerical error in the complementary filter
+    if (!is_equal(target_filter_hz,rangefinder_state.filter_hz)) {
+        // when changing target freq we use a reset to prevent a numerical error in the complementary filter
         rangefinder_state.filter_hz = target_filter_hz;
         rangefinder_state.alt_cm_filt.set_cutoff_frequency(rangefinder_state.filter_hz);
         rangefinder_state.alt_cm_filt.reset();
     }
 
     // apply complementary filter
-    rangefinder_state.alt_cm_filt.set_cutoff_frequency(rangefinder_state.filter_hz);
-    float rangefinder_alt_flt = rangefinder_state.alt_cm_filt.apply(rangefinder_alt_meas, inertial_nav.get_altitude(), AP_HAL::micros());
+    if (rangefinder_alt_meas_valid) {
+        rangefinder_state.alt_cm_filt.apply(rangefinder_alt_meas, inertial_nav.get_altitude(), AP_HAL::micros());
+    }
+    const float rangefinder_alt_flt = rangefinder_state.alt_cm_filt.get();
 
     // temporary debug log message to look at the
     // performance of the complementary filter