diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h
index 27a9120845..ab82df477e 100644
--- a/libraries/AP_Baro/AP_Baro.h
+++ b/libraries/AP_Baro/AP_Baro.h
@@ -30,6 +30,10 @@ class AP_Baro
     // of the last calibrate() call
     float get_altitude(void);
 
+    // return how many pressure samples were used to obtain
+    // the last pressure reading
+    uint8_t get_pressure_samples(void) { return _pressure_samples; }
+
     // get current climb rate in meters/s. A positive number means
     // going up
     float get_climb_rate(void);
@@ -42,6 +46,7 @@ class AP_Baro
 
 protected:
     uint32_t _last_update;
+    uint8_t _pressure_samples;
 
 private:
     AP_Int16    _ground_temperature;
diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h
index d706213626..a0c0dc49ed 100644
--- a/libraries/AP_Baro/AP_Baro_BMP085.h
+++ b/libraries/AP_Baro/AP_Baro_BMP085.h
@@ -11,7 +11,9 @@ class AP_Baro_BMP085 : public AP_Baro
 {
   public:
 	AP_Baro_BMP085(bool apm2_hardware):
-			_apm2_hardware(apm2_hardware) {};  // Constructor
+        _apm2_hardware(apm2_hardware) {
+             _pressure_samples = 1;
+         };  // Constructor
 
 
     /* AP_Baro public interface: */
diff --git a/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp b/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp
index 87ef1a7ad5..6dee39cfd2 100644
--- a/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp
+++ b/libraries/AP_Baro/AP_Baro_BMP085_hil.cpp
@@ -1,16 +1,7 @@
 /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
-extern "C" {
-  // AVR LibC Includes
-  #include <inttypes.h>
-  #include <avr/interrupt.h>
-}
-#if defined(ARDUINO) && ARDUINO >= 100
-  #include "Arduino.h"
-#else
-  #include "WConstants.h"
-#endif
-
+#include <FastSerial.h>
+#include <AP_Baro.h>
 #include "AP_Baro_BMP085_hil.h"
 
 // Constructors ////////////////////////////////////////////////////////////////
@@ -32,26 +23,33 @@ uint8_t AP_Baro_BMP085_HIL::read()
 {
 	uint8_t result = 0;
 
-	if (BMP085_State == 1){
-		BMP085_State++;
-	}else{
-
-		if (BMP085_State == 5){
-			BMP085_State = 1;				// Start again from state = 1
-			result = 1;						// New pressure reading
-		}else{
-			BMP085_State++;
-			result = 1;						// New pressure reading
-		}
+    if (_count != 0) {
+        result = 1;
+        Press = _pressure_sum / _count;
+        Temp = _temperature_sum / _count;
+        _pressure_samples = _count;
+        _count = 0;
+        _pressure_sum = 0;
+        _temperature_sum = 0;
 	}
-	return(result);
+
+	return result;
 }
 
 void AP_Baro_BMP085_HIL::setHIL(float _Temp, float _Press)
 {
-    // TODO: map floats to raw
-	Temp 	= _Temp;
-	Press 	= _Press;
+    _count++;
+    _pressure_sum += _Press;
+    _temperature_sum += _Temp;
+    if (_count == 128) {
+        // we have summed 128 values. This only happens
+        // when we stop reading the barometer for a long time
+        // (more than 1.2 seconds)
+        _count = 64;
+        _pressure_sum /= 2;
+        _temperature_sum /= 2;
+    }
+
 	healthy = true;
     _last_update = millis();
 }
diff --git a/libraries/AP_Baro/AP_Baro_BMP085_hil.h b/libraries/AP_Baro/AP_Baro_BMP085_hil.h
index cadb3813e9..42951d0b97 100644
--- a/libraries/AP_Baro/AP_Baro_BMP085_hil.h
+++ b/libraries/AP_Baro/AP_Baro_BMP085_hil.h
@@ -9,12 +9,14 @@ class AP_Baro_BMP085_HIL : public AP_Baro
 {
 private:
 	uint8_t BMP085_State;
-    int16_t Temp;
+    int32_t Temp;
     int32_t Press;
-    
+    int32_t _pressure_sum;
+    int32_t _temperature_sum;
+    uint8_t _count;
+
 public:
     AP_Baro_BMP085_HIL();  // Constructor
-	//int Altitude;
 
 	bool init(AP_PeriodicProcess * scheduler);
 	uint8_t read();
diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp
index 1e9ccbba72..1d301ec7c4 100644
--- a/libraries/AP_Baro/AP_Baro_MS5611.cpp
+++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp
@@ -175,7 +175,7 @@ void AP_Baro_MS5611::_update(uint32_t tnow)
 	    _s_D2 += _spi_read_adc();  				 // On state 0 we read temp
         _d2_count++;
         if (_d2_count == 32) {
-            // we have summed 128 values. This only happens
+            // we have summed 32 values. This only happens
             // when we stop reading the barometer for a long time
             // (more than 1.2 seconds)
             _s_D2 >>= 1;
@@ -226,6 +226,7 @@ uint8_t AP_Baro_MS5611::read()
         if (d2count != 0) {
             D2 = sD2 / d2count;
         }
+        _pressure_samples = d1count;
         _raw_press = D1;
         _raw_temp = D2;
     }