diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp
index e6755ffdc1..364f23b4eb 100644
--- a/libraries/AP_Baro/AP_Baro_BMP085.cpp
+++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp
@@ -173,29 +173,7 @@ void AP_Baro_BMP085::ReadPress()
 
 	RawPress = (((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
 
-	//if (_offset_press == 0){
-	//	_offset_press = RawPress;
-	//	RawPress = 0;
-	//} else{
-	//	RawPress -= _offset_press;
-	//}
-
-	// filter
-	//_press_filter[_press_index++] = RawPress;
-
-	//if(_press_index >= PRESS_FILTER_SIZE)
-	//	_press_index = 0;
-
-	//RawPress = 0;
-
-	// sum our filter
-	//for (uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){
-	//	RawPress += _press_filter[i];
-	//}
-
-	// grab result
-	//RawPress /= PRESS_FILTER_SIZE;
-	//RawPress += _offset_press;
+	RawPress = _press_filter.apply(RawPress);
 }
 
 // Send Command to Read Temperature
@@ -225,25 +203,21 @@ void AP_Baro_BMP085::ReadTemp()
 	_temp_sensor = buf[0];
 	_temp_sensor = (_temp_sensor << 8) | buf[1];
 
-	if (RawTemp == 0){
-		RawTemp = _temp_sensor;
-	}else{
-		RawTemp = (float)_temp_sensor * .01 + (float)RawTemp * .99;
-	}
+	RawTemp = _temp_filter.apply(_temp_sensor);
 }
 
 // Calculate Temperature and Pressure in real units.
 void AP_Baro_BMP085::Calculate()
 {
-	long x1, x2, x3, b3, b5, b6, p;
-	unsigned long b4, b7;
+	int32_t x1, x2, x3, b3, b5, b6, p;
+	uint32_t b4, b7;
 	int32_t tmp;
 
 	// See Datasheet page 13 for this formulas
 	// Based also on Jee Labs BMP085 example code. Thanks for share.
 	// Temperature calculations
-	x1 = ((long)RawTemp - ac6) * ac5 >> 15;
-	x2 = ((long) mc << 11) / (x1 + md);
+	x1 = ((int32_t)RawTemp - ac6) * ac5 >> 15;
+	x2 = ((int32_t) mc << 11) / (x1 + md);
 	b5 = x1 + x2;
 	Temp = (b5 + 8) >> 4;
 
diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h
index b7b329edd9..6e0d624925 100644
--- a/libraries/AP_Baro/AP_Baro_BMP085.h
+++ b/libraries/AP_Baro/AP_Baro_BMP085.h
@@ -5,14 +5,13 @@
 #define PRESS_FILTER_SIZE 2
 
 #include "AP_Baro.h"
+#include <AverageFilter.h>
 
 class AP_Baro_BMP085 : public AP_Baro
 {
   public:
 	AP_Baro_BMP085(bool apm2_hardware):
-			_apm2_hardware(apm2_hardware),
-			_temp_index(0),
-			_press_index(0){};  // Constructor
+			_apm2_hardware(apm2_hardware) {};  // Constructor
 
 
     /* AP_Baro public interface: */
@@ -40,10 +39,9 @@ class AP_Baro_BMP085 : public AP_Baro
 	int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
     uint16_t ac4, ac5, ac6;
 
-	int32_t _press_filter[PRESS_FILTER_SIZE];
+	AverageFilterInt32_Size2 _press_filter;
+	AverageFilterInt16_Size4 _temp_filter;
 
-	uint8_t	_temp_index;
-	uint8_t	_press_index;
     uint32_t _retry_time;
 
 	void Command_ReadPress();
diff --git a/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde b/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde
index 7bd8b4b18a..3cc61c9058 100644
--- a/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde
+++ b/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde
@@ -12,6 +12,7 @@
 #include <AP_InertialSensor.h>
 #include <AP_Math.h>
 #include <AP_Common.h>
+#include <AverageFilter.h>
 
 #ifndef APM2_HARDWARE
 # define APM2_HARDWARE 0