From 694bfc5b87dc5c3cb42c6f7b70e4d846b2616d25 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   | 38 ++++++++++++++-------
 2 files changed, 75 insertions(+), 15 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,33 +1,47 @@
 #ifndef APM_BMP085_h
 #define APM_BMP085_h
 
+#define TEMP_FILTER_SIZE 16
+#define PRESS_FILTER_SIZE 8
 
 class APM_BMP085_Class
 {
+  public:
+	APM_BMP085_Class():
+			_temp_index(0),
+			_press_index(0){};  // Constructor
+	int32_t RawPress;
+	int32_t RawTemp;
+	int16_t Temp;
+	int32_t Press;
+	//int Altitude;
+	uint8_t oss;
+	//int32_t Press0;  // Pressure at sea level
+
+	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();
 
-  public:
-	int32_t RawPress;
-	int32_t RawTemp;
-	int16_t Temp;
-	int32_t Press;
-	//int Altitude;
-	uint8_t oss;
-	//int32_t Press0;  // Pressure at sea level
-
-	APM_BMP085_Class();  // Constructor
-	void Init(int initialiseWireLib = 1);
-	uint8_t Read();
 };
 
 class APM_BMP085_HIL_Class