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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

225
libraries/APM_BMP085/APM_BMP085.cpp
View File

@ -9,21 +9,21 @@
Sensor is conected to I2C port
Sensor End of Conversion (EOC) pin is PC7 (30)
Variables:
RawTemp : Raw temperature data
RawPress : Raw pressure data
Temp : Calculated temperature (in 0.1<EFBFBD>C units)
Press : Calculated pressure (in Pa units)
Methods:
Init() : Initialization of I2C and read sensor calibration data
Read() : Read sensor data and calculate Temperature and Pressure
This function is optimized so the main host don<EFBFBD>t need to wait
This function is optimized so the main host don<EFBFBD>t need to wait
You can call this function in your main loop
It returns a 1 if there are new data.
Internal functions:
Command_ReadTemp(): Send commando to read temperature
Command_ReadPress(): Send commando to read Pressure
@ -31,7 +31,7 @@
ReadPress() : Read press register
Calculate() : Calculate Temperature and Pressure in real units
*/
extern "C" {
// AVR LibC Includes
@ -54,43 +54,45 @@ APM_BMP085_Class::APM_BMP085_Class()
// Public Methods //////////////////////////////////////////////////////////////
void APM_BMP085_Class::Init(int initialiseWireLib)
{
byte buff[22];
int i=0;
byte buff[22];
int i = 0;
pinMode(BMP085_EOC,INPUT); // End Of Conversion (PC7) input
if( initialiseWireLib != 0 )
Wire.begin();
oss = 3; // Over Sampling setting 3 = High resolution
BMP085_State = 0; // Initial state
pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input
if( initialiseWireLib != 0 )
Wire.begin();
oss = 3; // Over Sampling setting 3 = High resolution
BMP085_State = 0; // Initial state
// We read the calibration data registers
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xAA);
Wire.endTransmission();
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xAA);
Wire.endTransmission();
Wire.requestFrom(BMP085_ADDRESS, 22);
Wire.requestFrom(BMP085_ADDRESS, 22);
//Wire.endTransmission();
while(Wire.available())
{
buff[i] = Wire.receive(); // receive one byte
i++;
}
ac1 = ((int)buff[0] << 8) | buff[1];
ac2 = ((int)buff[2] << 8) | buff[3];
ac3 = ((int)buff[4] << 8) | buff[5];
ac4 = ((int)buff[6] << 8) | buff[7];
ac5 = ((int)buff[8] << 8) | buff[9];
ac6 = ((int)buff[10] << 8) | buff[11];
b1 = ((int)buff[12] << 8) | buff[13];
b2 = ((int)buff[14] << 8) | buff[15];
mb = ((int)buff[16] << 8) | buff[17];
mc = ((int)buff[18] << 8) | buff[19];
md = ((int)buff[20] << 8) | buff[21];
while(Wire.available()){
buff[i] = Wire.receive(); // receive one byte
i++;
}
ac1 = ((int)buff[0] << 8) | buff[1];
ac2 = ((int)buff[2] << 8) | buff[3];
ac3 = ((int)buff[4] << 8) | buff[5];
ac4 = ((int)buff[6] << 8) | buff[7];
ac5 = ((int)buff[8] << 8) | buff[9];
ac6 = ((int)buff[10] << 8) | buff[11];
b1 = ((int)buff[12] << 8) | buff[13];
b2 = ((int)buff[14] << 8) | buff[15];
mb = ((int)buff[16] << 8) | buff[17];
mc = ((int)buff[18] << 8) | buff[19];
md = ((int)buff[20] << 8) | buff[21];
//Send a command to read Temp
Command_ReadTemp();
BMP085_State=1;
Command_ReadTemp();
BMP085_State = 1;
}
@ -98,80 +100,78 @@ void APM_BMP085_Class::Init(int initialiseWireLib)
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
uint8_t APM_BMP085_Class::Read()
{
uint8_t result=0;
uint8_t result = 0;
if (BMP085_State==1)
{
if (digitalRead(BMP085_EOC))
{
ReadTemp(); // On state 1 we read temp
if (BMP085_State == 1){
if (digitalRead(BMP085_EOC)){
ReadTemp(); // On state 1 we read temp
BMP085_State++;
Command_ReadPress();
}
}
else
{
if (BMP085_State==5)
{
if (digitalRead(BMP085_EOC))
{
}else{
if (BMP085_State == 40){
if (digitalRead(BMP085_EOC)){
ReadPress();
Calculate();
BMP085_State = 1; // Start again from state=1
Command_ReadTemp(); // Read Temp
result=1; // New pressure reading
BMP085_State = 1; // Start again from state = 1
Command_ReadTemp(); // Read Temp
result = 1; // New pressure reading
}
}
else
{
if (digitalRead(BMP085_EOC))
{
}else{
if (digitalRead(BMP085_EOC)){
ReadPress();
Calculate();
BMP085_State++;
Command_ReadPress();
result=1; // New pressure reading
result = 1; // New pressure reading
}
}
}
return(result);
return(result);
}
// Send command to Read Pressure
void APM_BMP085_Class::Command_ReadPress()
{
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF4);
Wire.send(0x34+(oss<<6)); //write_register(0xF4,0x34+(oversampling_setting<<6));
Wire.endTransmission();
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF4);
Wire.send(0x34+(oss << 6)); // write_register(0xF4, 0x34+(oversampling_setting << 6));
Wire.endTransmission();
}
// Read Raw Pressure values
void APM_BMP085_Class::ReadPress()
{
byte msb;
byte lsb;
byte xlsb;
byte msb;
byte lsb;
byte xlsb;
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF6);
Wire.endTransmission();
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF6);
Wire.endTransmission();
Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte
while(!Wire.available()) {
Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte
while(!Wire.available()) {
// waiting
}
msb = Wire.receive();
while(!Wire.available()) {
}
msb = Wire.receive();
while(!Wire.available()) {
// waiting
}
lsb = Wire.receive();
while(!Wire.available()) {
}
lsb = Wire.receive();
while(!Wire.available()) {
// waiting
}
xlsb = Wire.receive();
RawPress = (((long)msb<<16) | ((long)lsb<<8) | ((long)xlsb)) >> (8-oss);
}
xlsb = Wire.receive();
RawPress = (((long)msb << 16) | ((long)lsb << 8) | ((long)xlsb)) >> (8 - oss);
}
// Send Command to Read Temperature
@ -185,20 +185,22 @@ void APM_BMP085_Class::Command_ReadTemp()
// Read Raw Temperature values
void APM_BMP085_Class::ReadTemp()
{
byte tmp;
{
byte tmp;
int16_t tmp2;
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF6);
Wire.endTransmission();
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF6);
Wire.endTransmission();
Wire.beginTransmission(BMP085_ADDRESS);
Wire.requestFrom(BMP085_ADDRESS,2);
while(!Wire.available()); // wait
RawTemp = Wire.receive();
while(!Wire.available()); // wait
tmp = Wire.receive();
RawTemp = RawTemp<<8 | tmp;
Wire.beginTransmission(BMP085_ADDRESS);
Wire.requestFrom(BMP085_ADDRESS, 2);
while(!Wire.available()); // wait
tmp2 = Wire.receive();
while(!Wire.available()); // wait
tmp = Wire.receive();
RawTemp += tmp2 << 8 | tmp;
RawTemp = RawTemp >> 1;
}
// Calculate Temperature and Pressure in real units.
@ -207,7 +209,7 @@ void APM_BMP085_Class::Calculate()
long x1, x2, x3, b3, b5, b6, p;
unsigned long 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
@ -218,7 +220,7 @@ void APM_BMP085_Class::Calculate()
// Pressure calculations
b6 = b5 - 4000;
x1 = (b2 * (b6 * b6 >> 12)) >> 11;
x1 = (b2 * (b6 * b6 >> 12)) >> 11;
x2 = ac2 * b6 >> 11;
x3 = x1 + x2;
//b3 = (((int32_t) ac1 * 4 + x3)<<oss + 2) >> 2; // BAD
@ -232,7 +234,7 @@ void APM_BMP085_Class::Calculate()
b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15;
b7 = ((uint32_t) RawPress - b3) * (50000 >> oss);
p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
x1 = (p >> 8) * (p >> 8);
x1 = (x1 * 3038) >> 16;
x2 = (-7357 * p) >> 16;
@ -252,34 +254,29 @@ void APM_BMP085_HIL_Class::Init(int initialiseWireLib)
// Read the sensor. This is a state machine
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
// We read one time Temperature (state = 1) and then 4 times Pressure (states 2-5)
uint8_t APM_BMP085_HIL_Class::Read()
{
uint8_t result=0;
uint8_t result = 0;
if (BMP085_State==1)
{
BMP085_State++;
}
else
{
if (BMP085_State==5)
{
BMP085_State = 1; // Start again from state=1
result=1; // New pressure reading
}
else
{
BMP085_State++;
result=1; // New pressure reading
if (BMP085_State == 1){
BMP085_State++;
}else{
if (BMP085_State == 5){
BMP085_State = 1; // Start again from state = 1
result = 1; // New pressure reading
}else{
BMP085_State++;
result = 1; // New pressure reading
}
}
return(result);
return(result);
}
void APM_BMP085_HIL_Class::setHIL(float _Temp, float _Press)
{
// TODO: map floats to raw
Temp = _Temp;
Press = _Press;
Temp = _Temp;
Press = _Press;
}

7
libraries/APM_BMP085/APM_BMP085.h
View File

@ -9,12 +9,13 @@ class APM_BMP085_Class
uint8_t BMP085_State;
// Internal calibration registers
int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
uint16_t ac4, ac5, ac6;
uint16_t ac4, ac5, ac6;
void Command_ReadPress();
void Command_ReadTemp();
void ReadPress();
void ReadTemp();
void Calculate();
public:
int32_t RawPress;
int32_t RawTemp;
@ -26,7 +27,7 @@ class APM_BMP085_Class
APM_BMP085_Class(); // Constructor
void Init(int initialiseWireLib = 1);
uint8_t Read();
uint8_t Read();
};
class APM_BMP085_HIL_Class
@ -42,7 +43,7 @@ class APM_BMP085_HIL_Class
uint8_t oss;
APM_BMP085_HIL_Class(); // Constructor
void Init(int initialiseWireLib = 1);
uint8_t Read();
uint8_t Read();
void setHIL(float Temp, float Press);
};