From cb8e9ef07b85ff77a3d391c6364631ffeb6aa105 Mon Sep 17 00:00:00 2001
From: pip <pip>
Date: Wed, 5 Jan 2011 11:26:47 +0000
Subject: [PATCH] Added a moving-average-filter to the pressure reading, and
 increased BMP085 oversampling from 2 to 3

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2336 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 flight/OpenPilot/Modules/Altitude/altitude.c | 33 ++++++++++++++++++--
 flight/PiOS/Boards/STM3210E_OP.h             |  3 +-
 2 files changed, 33 insertions(+), 3 deletions(-)

diff --git a/flight/OpenPilot/Modules/Altitude/altitude.c b/flight/OpenPilot/Modules/Altitude/altitude.c
index 17d54a16a..554269976 100644
--- a/flight/OpenPilot/Modules/Altitude/altitude.c
+++ b/flight/OpenPilot/Modules/Altitude/altitude.c
@@ -39,6 +39,8 @@
 #include "openpilot.h"
 #include "baroaltitude.h"	// object that will be updated by the module
 
+#define ALT_PRES_MAF        // uncommment this to not use the pressure moving-average-filter
+
 // Private constants
 #define STACK_SIZE configMINIMAL_STACK_SIZE
 #define TASK_PRIORITY (tskIDLE_PRIORITY+3)
@@ -49,6 +51,13 @@
 // Private variables
 static xTaskHandle taskHandle;
 
+// moving average filter variables
+#ifdef ALT_PRES_MAF
+    #define alt_maf_size    4
+    static int32_t alt_maf_buf[alt_maf_size];
+    static int32_t alt_maf_out = 0;
+#endif
+
 // Private functions
 static void altitudeTask(void *parameters);
 
@@ -61,6 +70,11 @@ int32_t AltitudeInitialize()
 	// Start main task
 	xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
 
+	// clear the moving average filter
+    for (int i = 0; i < alt_maf_size; i++)
+        alt_maf_buf[i] = 0;
+    alt_maf_out = 0;
+
 	return 0;
 }
 
@@ -89,9 +103,24 @@ static void altitudeTask(void *parameters)
 		PIOS_BMP085_StartADC(PressureConv);
 		xSemaphoreTake(PIOS_BMP085_EOC, portMAX_DELAY);
 
+		// read pressure
 		PIOS_BMP085_ReadADC();
-		// Convert from Pa to kPa
-		data.Pressure = PIOS_BMP085_GetPressure() / 1000.0;
+        int32_t pressure = PIOS_BMP085_GetPressure();
+
+        #ifdef ALT_PRES_MAF
+            // moving average filter the pressure
+            alt_maf_out -= alt_maf_buf[0];
+            for (int i = 0; i < alt_maf_size - 1; i++)
+                alt_maf_buf[i] = alt_maf_buf[i + 1];
+            alt_maf_buf[alt_maf_size - 1] = pressure;
+            alt_maf_out += pressure;
+
+            // Convert from Pa to kPa
+            data.Pressure = alt_maf_out / (1000.0f * alt_maf_size);
+        #else
+            // Convert from Pa to kPa
+            data.Pressure = pressure / 1000.0f;
+        #endif
 
 		// Compute the current altitude (all pressures in kPa)
 		data.Altitude = 44330.0 * (1.0 - powf((data.Pressure / (BMP085_P0 / 1000.0)), (1.0 / 5.255)));
diff --git a/flight/PiOS/Boards/STM3210E_OP.h b/flight/PiOS/Boards/STM3210E_OP.h
index c2ee9daa6..e26731d01 100644
--- a/flight/PiOS/Boards/STM3210E_OP.h
+++ b/flight/PiOS/Boards/STM3210E_OP.h
@@ -117,7 +117,8 @@ TIM8  | Servo 5   | Servo 6   | Servo 7   | Servo 8
 #define PIOS_BMP085_EOC_EXTI_LINE               EXTI_Line15
 #define PIOS_BMP085_EOC_IRQn                    EXTI15_10_IRQn
 #define PIOS_BMP085_EOC_PRIO                    PIOS_IRQ_PRIO_HIGH
-#define PIOS_BMP085_OVERSAMPLING                2
+//#define PIOS_BMP085_OVERSAMPLING                2
+#define PIOS_BMP085_OVERSAMPLING                3
 
 //-------------------------
 // USART