From 486c56ce417999c559433e4a30e1f919a748646c Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@mac.com>
Date: Thu, 8 Mar 2012 23:13:04 -0800
Subject: [PATCH] Consolidated Barometer pressure sensing to a single filter
 based on Randy's new filter class.

---
 ArduCopter/ArduCopter.pde            | 11 +++++---
 ArduCopter/sensors.pde               | 22 ++++++++++-----
 libraries/AP_Baro/AP_Baro_BMP085.cpp | 40 +++++++++++++++-------------
 3 files changed, 46 insertions(+), 27 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index ad3dcefb28..1a31602067 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -502,10 +502,14 @@ static int32_t initial_simple_bearing;
 // Used to control Axis lock
 int32_t roll_axis;
 int32_t pitch_axis;
+
 // Filters
 AverageFilterInt32_Size3 roll_rate_d_filter;	// filtered acceleration
 AverageFilterInt32_Size3 pitch_rate_d_filter;	// filtered pitch acceleration
 
+// Barometer filter
+AverageFilterInt32_Size5 baro_filter;	// filtered pitch acceleration
+
 ////////////////////////////////////////////////////////////////////////////////
 // Circle Mode / Loiter control
 ////////////////////////////////////////////////////////////////////////////////
@@ -1070,8 +1074,8 @@ static void medium_loop()
 			// -----------------------
 			arm_motors();
 
-			// Do an extra baro read
-			// ---------------------
+			// Do an extra baro read for Temp sensing
+			// ---------------------------------------
 			#if HIL_MODE != HIL_MODE_ATTITUDE
 			barometer.read();
 			#endif
@@ -1884,7 +1888,8 @@ static void update_altitude()
 		// This is real life
 
 		// read in Actual Baro Altitude
-		baro_alt 			= (baro_alt + read_barometer()) >> 1;
+		baro_alt 			= read_barometer();
+		//Serial.printf("baro_alt: %d \n", baro_alt);
 
 		// calc the vertical accel rate
 		int temp			= (baro_alt - old_baro_alt) * 10;
diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde
index 4c901b6357..cb0da8bb03 100644
--- a/ArduCopter/sensors.pde
+++ b/ArduCopter/sensors.pde
@@ -35,7 +35,7 @@ static void init_barometer(void)
 		//Serial.printf("init %ld, %d, -, %ld, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress,  barometer.Press);
 	}
 
-	for(i = 0; i < 20; i++){
+	for(i = 0; i < 40; i++){
 		delay(20);
 
 		#if HIL_MODE == HIL_MODE_SENSORS
@@ -44,15 +44,25 @@ static void init_barometer(void)
 
 		// Get initial data from absolute pressure sensor
 		barometer.read();
-		ground_pressure = barometer.get_pressure();
-		ground_temperature	= (ground_temperature * 7 + barometer.get_temperature()) / 8;
-		//Serial.printf("init %ld, %d, -, %ld, %ld, -, %d, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress,  barometer.Press, ground_temperature, ground_pressure);
+		ground_pressure 	= baro_filter.apply(barometer.get_pressure());
+
+		//Serial.printf("t: %ld, p: %d\n", ground_pressure, ground_temperature);
+
+		/*Serial.printf("init %d, %d, -, %d, %d, -, %d, %d\n",
+				barometer.RawTemp,
+				barometer.Temp,
+				barometer.RawPress,
+				barometer.Press,
+				ground_temperature,
+				ground_pressure);*/
 	}
+	// save our ground temp
+	ground_temperature	= barometer.get_temperature();
 }
 
 static void reset_baro(void)
 {
-	ground_pressure 	= barometer.get_pressure();
+	ground_pressure 	= baro_filter.apply(barometer.get_pressure());
 	ground_temperature	= barometer.get_temperature();
 }
 
@@ -61,7 +71,7 @@ static int32_t read_barometer(void)
  	float x, scaling, temp;
 
 	barometer.read();
-	float abs_pressure = barometer.get_pressure();
+	float abs_pressure = baro_filter.apply(barometer.get_pressure());
 
 
 	//Serial.printf("%ld, %ld, %ld, %ld\n", barometer.RawTemp, barometer.RawPress, barometer.Press, abs_pressure);
diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp
index d5790feb0d..e6755ffdc1 100644
--- a/libraries/AP_Baro/AP_Baro_BMP085.cpp
+++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp
@@ -94,6 +94,9 @@ bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
 	Command_ReadTemp();
 	BMP085_State = 1;
 
+	// init raw temo
+	RawTemp = 0;
+
 	healthy = true;
 	return true;
 }
@@ -170,29 +173,29 @@ 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;
-	}
+	//if (_offset_press == 0){
+	//	_offset_press = RawPress;
+	//	RawPress = 0;
+	//} else{
+	//	RawPress -= _offset_press;
+	//}
 
 	// filter
-	_press_filter[_press_index++] = RawPress;
+	//_press_filter[_press_index++] = RawPress;
 
-	if(_press_index >= PRESS_FILTER_SIZE)
-		_press_index = 0;
+	//if(_press_index >= PRESS_FILTER_SIZE)
+	//	_press_index = 0;
 
-	RawPress = 0;
+	//RawPress = 0;
 
 	// sum our filter
-	for (uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){
-		RawPress += _press_filter[i];
-	}
+	//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_SIZE;
+	//RawPress += _offset_press;
 }
 
 // Send Command to Read Temperature
@@ -222,10 +225,11 @@ void AP_Baro_BMP085::ReadTemp()
 	_temp_sensor = buf[0];
 	_temp_sensor = (_temp_sensor << 8) | buf[1];
 
-	if (RawTemp == 0)
+	if (RawTemp == 0){
 		RawTemp = _temp_sensor;
-
-	RawTemp = (float)_temp_sensor * .01 + (float)RawTemp * .99;
+	}else{
+		RawTemp = (float)_temp_sensor * .01 + (float)RawTemp * .99;
+	}
 }
 
 // Calculate Temperature and Pressure in real units.