diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp
index 180132a897..0db71329e8 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.cpp
@@ -77,7 +77,7 @@ bool AP_RangeFinder_PX4::take_reading(void)
     // try to accumulate one more sample, so we have the latest data
     accumulate();
 
-    // consider the compass healthy if we got a reading in the last 0.2s
+    // consider the range finder healthy if we got a reading in the last 0.2s
     for (uint8_t i=0; i<_num_instances; i++) {
         _healthy[i] = (hrt_absolute_time() - _last_timestamp[i] < 200000);
     }
@@ -89,7 +89,12 @@ bool AP_RangeFinder_PX4::take_reading(void)
         _sum[i] /= _count[i];
         _sum[i] *= 100.00f;
     
-        _distance[i] = _mode_filter->apply(_sum[i]);
+        if (_mode_filter) {
+            _distance[i] = _mode_filter->apply(_sum[i]);
+        }
+        else {
+            _distance[i] = _sum[i];
+        }
     
         _sum[i] = 0;
         _count[i] = 0;
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h
index f2c08f69a6..a7a1a9114a 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_PX4.h
@@ -23,7 +23,7 @@ class AP_RangeFinder_PX4 : public RangeFinder
 {
 public:
     // constructor
-    AP_RangeFinder_PX4(FilterInt16 *filter);
+    AP_RangeFinder_PX4(FilterInt16 *);
     
     // initialize all the range finder devices
     bool init(void);
diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index 9188d42859..064db169a6 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/RangeFinder.cpp
@@ -36,7 +36,12 @@ int16_t RangeFinder::read()
     // ensure distance is within min and max
     temp_dist = constrain_float(temp_dist, min_distance, max_distance);
 
-    distance = _mode_filter->apply(temp_dist);
+    if (_mode_filter) {
+        distance = _mode_filter->apply(temp_dist);
+    }
+    else {
+        distance = temp_dist;
+    }
     return distance;
 }