From ed0d1da4f5a477b5aec8c707ce1e34957e6bed2a Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Sun, 18 Mar 2012 01:06:02 +0900
Subject: [PATCH] AP_Baro - added average filter for temperature to replace
broken filter - added average filter (for last two values) for raw
pressure - changed some "long" to int32_t and "unsigned long" to
uint32_t
---
libraries/AP_Baro/AP_Baro_BMP085.cpp | 38 +++----------------
libraries/AP_Baro/AP_Baro_BMP085.h | 10 ++---
.../AP_Baro_BMP085_test.pde | 1 +
3 files changed, 11 insertions(+), 38 deletions(-)
diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp
index e6755ffdc..364f23b4e 100644
--- a/libraries/AP_Baro/AP_Baro_BMP085.cpp
+++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp
@@ -173,29 +173,7 @@ 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;
- //}
-
- // 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 += _offset_press;
+ RawPress = _press_filter.apply(RawPress);
}
// Send Command to Read Temperature
@@ -225,25 +203,21 @@ void AP_Baro_BMP085::ReadTemp()
_temp_sensor = buf[0];
_temp_sensor = (_temp_sensor << 8) | buf[1];
- if (RawTemp == 0){
- RawTemp = _temp_sensor;
- }else{
- RawTemp = (float)_temp_sensor * .01 + (float)RawTemp * .99;
- }
+ RawTemp = _temp_filter.apply(_temp_sensor);
}
// Calculate Temperature and Pressure in real units.
void AP_Baro_BMP085::Calculate()
{
- long x1, x2, x3, b3, b5, b6, p;
- unsigned long b4, b7;
+ int32_t x1, x2, x3, b3, b5, b6, p;
+ uint32_t b4, b7;
int32_t tmp;
// See Datasheet page 13 for this formulas
// Based also on Jee Labs BMP085 example code. Thanks for share.
// Temperature calculations
- x1 = ((long)RawTemp - ac6) * ac5 >> 15;
- x2 = ((long) mc << 11) / (x1 + md);
+ x1 = ((int32_t)RawTemp - ac6) * ac5 >> 15;
+ x2 = ((int32_t) mc << 11) / (x1 + md);
b5 = x1 + x2;
Temp = (b5 + 8) >> 4;
diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h
index b7b329edd..6e0d62492 100644
--- a/libraries/AP_Baro/AP_Baro_BMP085.h
+++ b/libraries/AP_Baro/AP_Baro_BMP085.h
@@ -5,14 +5,13 @@
#define PRESS_FILTER_SIZE 2
#include "AP_Baro.h"
+#include <AverageFilter.h>
class AP_Baro_BMP085 : public AP_Baro
{
public:
AP_Baro_BMP085(bool apm2_hardware):
- _apm2_hardware(apm2_hardware),
- _temp_index(0),
- _press_index(0){}; // Constructor
+ _apm2_hardware(apm2_hardware) {}; // Constructor
/* AP_Baro public interface: */
@@ -40,10 +39,9 @@ class AP_Baro_BMP085 : public AP_Baro
int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
uint16_t ac4, ac5, ac6;
- int32_t _press_filter[PRESS_FILTER_SIZE];
+ AverageFilterInt32_Size2 _press_filter;
+ AverageFilterInt16_Size4 _temp_filter;
- uint8_t _temp_index;
- uint8_t _press_index;
uint32_t _retry_time;
void Command_ReadPress();
diff --git a/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde b/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde
index 7bd8b4b18..3cc61c905 100644
--- a/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde
+++ b/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde
@@ -12,6 +12,7 @@
#include <AP_InertialSensor.h>
#include <AP_Math.h>
#include <AP_Common.h>
+#include <AverageFilter.h>
#ifndef APM2_HARDWARE
# define APM2_HARDWARE 0
--
GitLab