Adjusted the state machine to read the temperature less often (from twice a second to every 4 seconds). Added a small smoothing filter.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2552 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-06-12 23:57:36 +00:00
parent c1ddd58d4c
commit 2651cadb84
2 changed files with 115 additions and 117 deletions

View File

@ -61,6 +61,7 @@ void APM_BMP085_Class::Init(int initialiseWireLib)
if( initialiseWireLib != 0 ) if( initialiseWireLib != 0 )
Wire.begin(); Wire.begin();
oss = 3; // Over Sampling setting 3 = High resolution oss = 3; // Over Sampling setting 3 = High resolution
BMP085_State = 0; // Initial state BMP085_State = 0; // Initial state
@ -70,12 +71,13 @@ void APM_BMP085_Class::Init(int initialiseWireLib)
Wire.endTransmission(); Wire.endTransmission();
Wire.requestFrom(BMP085_ADDRESS, 22); Wire.requestFrom(BMP085_ADDRESS, 22);
//Wire.endTransmission(); //Wire.endTransmission();
while(Wire.available()) while(Wire.available()){
{
buff[i] = Wire.receive(); // receive one byte buff[i] = Wire.receive(); // receive one byte
i++; i++;
} }
ac1 = ((int)buff[0] << 8) | buff[1]; ac1 = ((int)buff[0] << 8) | buff[1];
ac2 = ((int)buff[2] << 8) | buff[3]; ac2 = ((int)buff[2] << 8) | buff[3];
ac3 = ((int)buff[4] << 8) | buff[5]; ac3 = ((int)buff[4] << 8) | buff[5];
@ -100,32 +102,24 @@ uint8_t APM_BMP085_Class::Read()
{ {
uint8_t result = 0; uint8_t result = 0;
if (BMP085_State==1) if (BMP085_State == 1){
{ if (digitalRead(BMP085_EOC)){
if (digitalRead(BMP085_EOC))
{
ReadTemp(); // On state 1 we read temp ReadTemp(); // On state 1 we read temp
BMP085_State++; BMP085_State++;
Command_ReadPress(); Command_ReadPress();
} }
} }else{
else if (BMP085_State == 40){
{ if (digitalRead(BMP085_EOC)){
if (BMP085_State==5)
{
if (digitalRead(BMP085_EOC))
{
ReadPress(); ReadPress();
Calculate(); Calculate();
BMP085_State = 1; // Start again from state = 1 BMP085_State = 1; // Start again from state = 1
Command_ReadTemp(); // Read Temp Command_ReadTemp(); // Read Temp
result = 1; // New pressure reading result = 1; // New pressure reading
} }
} }else{
else if (digitalRead(BMP085_EOC)){
{
if (digitalRead(BMP085_EOC))
{
ReadPress(); ReadPress();
Calculate(); Calculate();
BMP085_State++; BMP085_State++;
@ -159,17 +153,23 @@ void APM_BMP085_Class::ReadPress()
Wire.endTransmission(); Wire.endTransmission();
Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte
while(!Wire.available()) { while(!Wire.available()) {
// waiting // waiting
} }
msb = Wire.receive(); msb = Wire.receive();
while(!Wire.available()) { while(!Wire.available()) {
// waiting // waiting
} }
lsb = Wire.receive(); lsb = Wire.receive();
while(!Wire.available()) { while(!Wire.available()) {
// waiting // waiting
} }
xlsb = Wire.receive(); xlsb = Wire.receive();
RawPress = (((long)msb << 16) | ((long)lsb << 8) | ((long)xlsb)) >> (8 - oss); RawPress = (((long)msb << 16) | ((long)lsb << 8) | ((long)xlsb)) >> (8 - oss);
} }
@ -187,6 +187,7 @@ void APM_BMP085_Class::Command_ReadTemp()
void APM_BMP085_Class::ReadTemp() void APM_BMP085_Class::ReadTemp()
{ {
byte tmp; byte tmp;
int16_t tmp2;
Wire.beginTransmission(BMP085_ADDRESS); Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF6); Wire.send(0xF6);
@ -195,10 +196,11 @@ void APM_BMP085_Class::ReadTemp()
Wire.beginTransmission(BMP085_ADDRESS); Wire.beginTransmission(BMP085_ADDRESS);
Wire.requestFrom(BMP085_ADDRESS, 2); Wire.requestFrom(BMP085_ADDRESS, 2);
while(!Wire.available()); // wait while(!Wire.available()); // wait
RawTemp = Wire.receive(); tmp2 = Wire.receive();
while(!Wire.available()); // wait while(!Wire.available()); // wait
tmp = Wire.receive(); tmp = Wire.receive();
RawTemp = RawTemp<<8 | tmp; RawTemp += tmp2 << 8 | tmp;
RawTemp = RawTemp >> 1;
} }
// Calculate Temperature and Pressure in real units. // Calculate Temperature and Pressure in real units.
@ -257,19 +259,14 @@ uint8_t APM_BMP085_HIL_Class::Read()
{ {
uint8_t result = 0; uint8_t result = 0;
if (BMP085_State==1) if (BMP085_State == 1){
{
BMP085_State++; BMP085_State++;
} }else{
else
{ if (BMP085_State == 5){
if (BMP085_State==5)
{
BMP085_State = 1; // Start again from state = 1 BMP085_State = 1; // Start again from state = 1
result = 1; // New pressure reading result = 1; // New pressure reading
} }else{
else
{
BMP085_State++; BMP085_State++;
result = 1; // New pressure reading result = 1; // New pressure reading
} }

View File

@ -15,6 +15,7 @@ class APM_BMP085_Class
void ReadPress(); void ReadPress();
void ReadTemp(); void ReadTemp();
void Calculate(); void Calculate();
public: public:
int32_t RawPress; int32_t RawPress;
int32_t RawTemp; int32_t RawTemp;