From 58e9a913a13c3e8d2b5f437c03e7ca9dc2ba2d90 Mon Sep 17 00:00:00 2001
From: jasonshort <jasonshort@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Sun, 4 Sep 2011 17:47:50 +0000
Subject: [PATCH] Added two filters to the RawTemp and RawPress values to
 increase accuracy from noisy temp sensor.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3242 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 libraries/APM_BMP085/APM_BMP085.cpp | 52 +++++++++++++++++++++++++++--
 libraries/APM_BMP085/APM_BMP085.h   | 40 ++++++++++++++--------
 2 files changed, 76 insertions(+), 16 deletions(-)

diff --git a/libraries/APM_BMP085/APM_BMP085.cpp b/libraries/APM_BMP085/APM_BMP085.cpp
index fd7749b871..fc9322d6df 100644
--- a/libraries/APM_BMP085/APM_BMP085.cpp
+++ b/libraries/APM_BMP085/APM_BMP085.cpp
@@ -47,9 +47,9 @@ extern "C" {
 #define BMP085_EOC 30        // End of conversion pin PC7
 
 // Constructors ////////////////////////////////////////////////////////////////
-APM_BMP085_Class::APM_BMP085_Class()
-{
-}
+//APM_BMP085_Class::APM_BMP085_Class()
+//{
+//}
 
 // Public Methods //////////////////////////////////////////////////////////////
 void APM_BMP085_Class::Init(int initialiseWireLib)
@@ -172,6 +172,29 @@ void APM_BMP085_Class::ReadPress()
 
 	xlsb = Wire.receive();
 	RawPress = (((long)msb << 16) | ((long)lsb << 8) | ((long)xlsb)) >> (8 - oss);
+
+	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 >>= 3;
+	RawPress += _offset_press;
 }
 
 // Send Command to Read Temperature
@@ -201,6 +224,29 @@ void APM_BMP085_Class::ReadTemp()
 	tmp 	= Wire.receive();
 
 	RawTemp = RawTemp << 8 | tmp;
+
+	if(_offset_temp == 0){
+		_offset_temp = RawTemp;
+		RawTemp = 0;
+	}else{
+		RawTemp -= _offset_temp;
+	}
+
+	// filter
+	_temp_filter[_temp_index++] = RawTemp;
+
+	if(_temp_index >= TEMP_FILTER_SIZE)
+		_temp_index = 0;
+
+	RawTemp = 0;
+	// sum our filter
+	for(uint8_t i = 0; i < TEMP_FILTER_SIZE; i++){
+		RawTemp += _temp_filter[i];
+	}
+
+	// grab result
+	RawTemp >>= 4;
+	RawTemp += _offset_temp;
 }
 
 // Calculate Temperature and Pressure in real units.
diff --git a/libraries/APM_BMP085/APM_BMP085.h b/libraries/APM_BMP085/APM_BMP085.h
index 5c9bc02cd3..23abe6ec68 100644
--- a/libraries/APM_BMP085/APM_BMP085.h
+++ b/libraries/APM_BMP085/APM_BMP085.h
@@ -1,22 +1,15 @@
 #ifndef APM_BMP085_h
 #define APM_BMP085_h
 
+#define TEMP_FILTER_SIZE 16
+#define PRESS_FILTER_SIZE 8
 
 class APM_BMP085_Class
 {
-  private:
-    // State machine
-    uint8_t BMP085_State;
-	// Internal calibration registers
-	int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
-    uint16_t ac4, ac5, ac6;
-	void Command_ReadPress();
-	void Command_ReadTemp();
-	void ReadPress();
-	void ReadTemp();
-	void Calculate();
-
   public:
+	APM_BMP085_Class():
+			_temp_index(0),
+			_press_index(0){};  // Constructor
 	int32_t RawPress;
 	int32_t RawTemp;
 	int16_t Temp;
@@ -25,9 +18,30 @@ class APM_BMP085_Class
 	uint8_t oss;
 	//int32_t Press0;  // Pressure at sea level
 
-	APM_BMP085_Class();  // Constructor
 	void Init(int initialiseWireLib = 1);
 	uint8_t Read();
+
+  private:
+    // State machine
+    uint8_t BMP085_State;
+	// Internal calibration registers
+	int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
+    uint16_t ac4, ac5, ac6;
+
+	int	 	_temp_filter[TEMP_FILTER_SIZE];
+	int	 	_press_filter[PRESS_FILTER_SIZE];
+	long	_offset_press;
+	long	_offset_temp;
+
+	uint8_t	_temp_index;
+	uint8_t	_press_index;
+
+	void Command_ReadPress();
+	void Command_ReadTemp();
+	void ReadPress();
+	void ReadTemp();
+	void Calculate();
+
 };
 
 class APM_BMP085_HIL_Class