Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
rmackay9 2012-03-18 01:06:02 +09:00
parent 427dacba70
commit 7560242721
3 changed files with 11 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_Baro/AP_Baro_BMP085.cpp
View File

@ -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); RawPress = (((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
//if (_offset_press == 0){ RawPress = _press_filter.apply(RawPress);
// _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;
} }
// Send Command to Read Temperature // Send Command to Read Temperature
@ -225,25 +203,21 @@ void AP_Baro_BMP085::ReadTemp()
_temp_sensor = buf[0]; _temp_sensor = buf[0];
_temp_sensor = (_temp_sensor << 8) | buf[1]; _temp_sensor = (_temp_sensor << 8) | buf[1];
if (RawTemp == 0){ RawTemp = _temp_filter.apply(_temp_sensor);
RawTemp = _temp_sensor;
}else{
RawTemp = (float)_temp_sensor * .01 + (float)RawTemp * .99;
}
} }
// Calculate Temperature and Pressure in real units. // Calculate Temperature and Pressure in real units.
void AP_Baro_BMP085::Calculate() void AP_Baro_BMP085::Calculate()
{ {
long x1, x2, x3, b3, b5, b6, p; int32_t x1, x2, x3, b3, b5, b6, p;
unsigned long b4, b7; uint32_t b4, b7;
int32_t tmp; int32_t tmp;
// See Datasheet page 13 for this formulas // See Datasheet page 13 for this formulas
// Based also on Jee Labs BMP085 example code. Thanks for share. // Based also on Jee Labs BMP085 example code. Thanks for share.
// Temperature calculations // Temperature calculations
x1 = ((long)RawTemp - ac6) * ac5 >> 15; x1 = ((int32_t)RawTemp - ac6) * ac5 >> 15;
x2 = ((long) mc << 11) / (x1 + md); x2 = ((int32_t) mc << 11) / (x1 + md);
b5 = x1 + x2; b5 = x1 + x2;
Temp = (b5 + 8) >> 4; Temp = (b5 + 8) >> 4;

10
libraries/AP_Baro/AP_Baro_BMP085.h
View File

@ -5,14 +5,13 @@
#define PRESS_FILTER_SIZE 2 #define PRESS_FILTER_SIZE 2
#include "AP_Baro.h" #include "AP_Baro.h"
#include <AverageFilter.h>
class AP_Baro_BMP085 : public AP_Baro class AP_Baro_BMP085 : public AP_Baro
{ {
public: public:
AP_Baro_BMP085(bool apm2_hardware): AP_Baro_BMP085(bool apm2_hardware):
_apm2_hardware(apm2_hardware), _apm2_hardware(apm2_hardware) {}; // Constructor
_temp_index(0),
_press_index(0){}; // Constructor
/* AP_Baro public interface: */ /* 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; int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
uint16_t ac4, ac5, ac6; 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; uint32_t _retry_time;
void Command_ReadPress(); void Command_ReadPress();

1
libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde
View File

@ -12,6 +12,7 @@
#include <AP_InertialSensor.h> #include <AP_InertialSensor.h>
#include <AP_Math.h> #include <AP_Math.h>
#include <AP_Common.h> #include <AP_Common.h>
#include <AverageFilter.h>
#ifndef APM2_HARDWARE #ifndef APM2_HARDWARE
# define APM2_HARDWARE 0 # define APM2_HARDWARE 0