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

BMP085 fix line endings

This commit is contained in:
Pat Hickey 2011-11-26 20:42:52 -08:00 committed by Pat Hickey
parent 7b51a2c42a
commit 917418994d
2 changed files with 357 additions and 357 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

620
libraries/AP_Baro/AP_Baro_BMP085.cpp
View File

@ -1,57 +1,57 @@
/* /*
APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor
Code by Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com Code by Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com
This library is free software; you can redistribute it and/or This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. version 2.1 of the License, or (at your option) any later version.
Sensor is conected to I2C port Sensor is conected to I2C port
Sensor End of Conversion (EOC) pin is PC7 (30) Sensor End of Conversion (EOC) pin is PC7 (30)
Variables: Variables:
RawTemp : Raw temperature data RawTemp : Raw temperature data
RawPress : Raw pressure data RawPress : Raw pressure data
Temp : Calculated temperature (in 0.1<EFBFBD>C units) Temp : Calculated temperature (in 0.1<EFBFBD>C units)
Press : Calculated pressure (in Pa units) Press : Calculated pressure (in Pa units)
Methods: Methods:
Init() : Initialization of I2C and read sensor calibration data Init() : Initialization of I2C and read sensor calibration data
Read() : Read sensor data and calculate Temperature and Pressure 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 You can call this function in your main loop
It returns a 1 if there are new data. It returns a 1 if there are new data.
Internal functions: Internal functions:
Command_ReadTemp(): Send commando to read temperature Command_ReadTemp(): Send commando to read temperature
Command_ReadPress(): Send commando to read Pressure Command_ReadPress(): Send commando to read Pressure
ReadTemp() : Read temp register ReadTemp() : Read temp register
ReadPress() : Read press register ReadPress() : Read press register
Calculate() : Calculate Temperature and Pressure in real units Calculate() : Calculate Temperature and Pressure in real units
*/ */
extern "C" {
// AVR LibC Includes
#include <inttypes.h>
#include <avr/interrupt.h>
#include "WConstants.h"
}
#include <Wire.h>
#include "APM_BMP085.h"
#define BMP085_ADDRESS 0x77 //(0xEE >> 1)
#define BMP085_EOC 30 // End of conversion pin PC7
// Constructors ////////////////////////////////////////////////////////////////
//APM_BMP085_Class::APM_BMP085_Class()
//{
//}
extern "C" {
// AVR LibC Includes
#include <inttypes.h>
#include <avr/interrupt.h>
#include "WConstants.h"
}
#include <Wire.h>
#include "APM_BMP085.h"
#define BMP085_ADDRESS 0x77 //(0xEE >> 1)
#define BMP085_EOC 30 // End of conversion pin PC7
// Constructors ////////////////////////////////////////////////////////////////
//APM_BMP085_Class::APM_BMP085_Class()
//{
//}
// the apm2 hardware needs to check the state of the // the apm2 hardware needs to check the state of the
// chip using a direct IO port // chip using a direct IO port
// On APM2 prerelease hw, the data ready port is hooked up to PE7, which // On APM2 prerelease hw, the data ready port is hooked up to PE7, which
@ -59,271 +59,271 @@ extern "C" {
#define BMP_DATA_READY() (_apm2_hardware?(PINE&0x80):digitalRead(BMP085_EOC)) #define BMP_DATA_READY() (_apm2_hardware?(PINE&0x80):digitalRead(BMP085_EOC))
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
bool APM_BMP085_Class::Init(int initialiseWireLib, bool apm2_hardware) bool APM_BMP085_Class::Init(int initialiseWireLib, bool apm2_hardware)
{ {
byte buff[22]; byte buff[22];
int i = 0; int i = 0;
_apm2_hardware = apm2_hardware; _apm2_hardware = apm2_hardware;
pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input
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
// We read the calibration data registers // We read the calibration data registers
Wire.beginTransmission(BMP085_ADDRESS); Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xAA); Wire.send(0xAA);
if (Wire.endTransmission() != 0) { if (Wire.endTransmission() != 0) {
return false; return false;
} }
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++;
} }
if (i != 22) { if (i != 22) {
return false; return false;
} }
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];
ac4 = ((int)buff[6] << 8) | buff[7]; ac4 = ((int)buff[6] << 8) | buff[7];
ac5 = ((int)buff[8] << 8) | buff[9]; ac5 = ((int)buff[8] << 8) | buff[9];
ac6 = ((int)buff[10] << 8) | buff[11]; ac6 = ((int)buff[10] << 8) | buff[11];
b1 = ((int)buff[12] << 8) | buff[13]; b1 = ((int)buff[12] << 8) | buff[13];
b2 = ((int)buff[14] << 8) | buff[15]; b2 = ((int)buff[14] << 8) | buff[15];
mb = ((int)buff[16] << 8) | buff[17]; mb = ((int)buff[16] << 8) | buff[17];
mc = ((int)buff[18] << 8) | buff[19]; mc = ((int)buff[18] << 8) | buff[19];
md = ((int)buff[20] << 8) | buff[21]; md = ((int)buff[20] << 8) | buff[21];
//Send a command to read Temp //Send a command to read Temp
Command_ReadTemp(); Command_ReadTemp();
BMP085_State = 1; BMP085_State = 1;
return true; return true;
} }
/* /*
// Read the sensor. This is a state machine // 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_Class::Read() uint8_t APM_BMP085_Class::Read()
{ {
uint8_t result = 0; uint8_t result = 0;
if (BMP085_State == 1){ if (BMP085_State == 1){
if (BMP_DATA_READY()) { if (BMP_DATA_READY()) {
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 == 5){ if (BMP085_State == 5){
if (BMP_DATA_READY()){ if (BMP_DATA_READY()){
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 (BMP_DATA_READY()){ if (BMP_DATA_READY()){
ReadPress(); ReadPress();
Calculate(); Calculate();
BMP085_State++; BMP085_State++;
Command_ReadPress(); Command_ReadPress();
result = 1; // New pressure reading result = 1; // New pressure reading
} }
} }
} }
return(result); return(result);
} }
*/ */
// Read the sensor. This is a state machine // 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_Class::Read() uint8_t APM_BMP085_Class::Read()
{ {
uint8_t result = 0; uint8_t result = 0;
if (BMP085_State == 1){ if (BMP085_State == 1){
if (BMP_DATA_READY()){ if (BMP_DATA_READY()){
BMP085_State = 2; BMP085_State = 2;
ReadTemp(); // On state 1 we read temp ReadTemp(); // On state 1 we read temp
Command_ReadPress(); Command_ReadPress();
} }
}else{ }else{
if (BMP_DATA_READY()){ if (BMP_DATA_READY()){
BMP085_State = 1; // Start again from state = 1 BMP085_State = 1; // Start again from state = 1
ReadPress(); ReadPress();
Calculate(); Calculate();
Command_ReadTemp(); // Read Temp Command_ReadTemp(); // Read Temp
result = 1; // New pressure reading result = 1; // New pressure reading
} }
} }
return(result); return(result);
} }
// Send command to Read Pressure // Send command to Read Pressure
void APM_BMP085_Class::Command_ReadPress() void APM_BMP085_Class::Command_ReadPress()
{ {
Wire.beginTransmission(BMP085_ADDRESS); Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF4); Wire.send(0xF4);
Wire.send(0x34+(oss << 6)); // write_register(0xF4, 0x34+(oversampling_setting << 6)); Wire.send(0x34+(oss << 6)); // write_register(0xF4, 0x34+(oversampling_setting << 6));
Wire.endTransmission(); Wire.endTransmission();
} }
// Read Raw Pressure values // Read Raw Pressure values
void APM_BMP085_Class::ReadPress() void APM_BMP085_Class::ReadPress()
{ {
byte msb; byte msb;
byte lsb; byte lsb;
byte xlsb; byte xlsb;
Wire.beginTransmission(BMP085_ADDRESS); Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF6); Wire.send(0xF6);
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);
if(_offset_press == 0){ if(_offset_press == 0){
_offset_press = RawPress; _offset_press = RawPress;
RawPress = 0; RawPress = 0;
}else{ }else{
RawPress -= _offset_press; RawPress -= _offset_press;
} }
// filter // filter
_press_filter[_press_index++] = RawPress; _press_filter[_press_index++] = RawPress;
if(_press_index >= PRESS_FILTER_SIZE) if(_press_index >= PRESS_FILTER_SIZE)
_press_index = 0; _press_index = 0;
RawPress = 0; RawPress = 0;
// sum our filter // sum our filter
for(uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){ for(uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){
RawPress += _press_filter[i]; RawPress += _press_filter[i];
} }
// grab result // grab result
RawPress /= PRESS_FILTER_SIZE; RawPress /= PRESS_FILTER_SIZE;
//RawPress >>= 3; //RawPress >>= 3;
RawPress += _offset_press; RawPress += _offset_press;
} }
// Send Command to Read Temperature // Send Command to Read Temperature
void APM_BMP085_Class::Command_ReadTemp() void APM_BMP085_Class::Command_ReadTemp()
{ {
Wire.beginTransmission(BMP085_ADDRESS); Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF4); Wire.send(0xF4);
Wire.send(0x2E); Wire.send(0x2E);
Wire.endTransmission(); Wire.endTransmission();
} }
// Read Raw Temperature values // Read Raw Temperature values
void APM_BMP085_Class::ReadTemp() void APM_BMP085_Class::ReadTemp()
{ {
byte tmp; byte tmp;
Wire.beginTransmission(BMP085_ADDRESS); Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF6); Wire.send(0xF6);
Wire.endTransmission(); Wire.endTransmission();
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(); RawTemp = Wire.receive();
while(!Wire.available()); // wait while(!Wire.available()); // wait
tmp = Wire.receive(); tmp = Wire.receive();
RawTemp = RawTemp << 8 | tmp; RawTemp = RawTemp << 8 | tmp;
if(_offset_temp == 0){ if(_offset_temp == 0){
_offset_temp = RawTemp; _offset_temp = RawTemp;
RawTemp = 0; RawTemp = 0;
}else{ }else{
RawTemp -= _offset_temp; RawTemp -= _offset_temp;
} }
// filter // filter
_temp_filter[_temp_index++] = RawTemp; _temp_filter[_temp_index++] = RawTemp;
if(_temp_index >= TEMP_FILTER_SIZE) if(_temp_index >= TEMP_FILTER_SIZE)
_temp_index = 0; _temp_index = 0;
RawTemp = 0; RawTemp = 0;
// sum our filter // sum our filter
for(uint8_t i = 0; i < TEMP_FILTER_SIZE; i++){ for(uint8_t i = 0; i < TEMP_FILTER_SIZE; i++){
RawTemp += _temp_filter[i]; RawTemp += _temp_filter[i];
} }
// grab result // grab result
RawTemp /= TEMP_FILTER_SIZE; RawTemp /= TEMP_FILTER_SIZE;
//RawTemp >>= 4; //RawTemp >>= 4;
RawTemp += _offset_temp; RawTemp += _offset_temp;
} }
// Calculate Temperature and Pressure in real units. // Calculate Temperature and Pressure in real units.
void APM_BMP085_Class::Calculate() void APM_BMP085_Class::Calculate()
{ {
long x1, x2, x3, b3, b5, b6, p; long x1, x2, x3, b3, b5, b6, p;
unsigned long b4, b7; unsigned long 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 = ((long)RawTemp - ac6) * ac5 >> 15;
x2 = ((long) mc << 11) / (x1 + md); x2 = ((long) mc << 11) / (x1 + md);
b5 = x1 + x2; b5 = x1 + x2;
Temp = (b5 + 8) >> 4; Temp = (b5 + 8) >> 4;
// Pressure calculations // Pressure calculations
b6 = b5 - 4000; b6 = b5 - 4000;
x1 = (b2 * (b6 * b6 >> 12)) >> 11; x1 = (b2 * (b6 * b6 >> 12)) >> 11;
x2 = ac2 * b6 >> 11; x2 = ac2 * b6 >> 11;
x3 = x1 + x2; x3 = x1 + x2;
//b3 = (((int32_t) ac1 * 4 + x3)<<oss + 2) >> 2; // BAD //b3 = (((int32_t) ac1 * 4 + x3)<<oss + 2) >> 2; // BAD
//b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for oss=0 //b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for oss=0
tmp = ac1; tmp = ac1;
tmp = (tmp*4 + x3)<<oss; tmp = (tmp*4 + x3)<<oss;
b3 = (tmp+2)/4; b3 = (tmp+2)/4;
x1 = ac3 * b6 >> 13; x1 = ac3 * b6 >> 13;
x2 = (b1 * (b6 * b6 >> 12)) >> 16; x2 = (b1 * (b6 * b6 >> 12)) >> 16;
x3 = ((x1 + x2) + 2) >> 2; x3 = ((x1 + x2) + 2) >> 2;
b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15; b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15;
b7 = ((uint32_t) RawPress - b3) * (50000 >> oss); b7 = ((uint32_t) RawPress - b3) * (50000 >> oss);
p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2; p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
x1 = (p >> 8) * (p >> 8); x1 = (p >> 8) * (p >> 8);
x1 = (x1 * 3038) >> 16; x1 = (x1 * 3038) >> 16;
x2 = (-7357 * p) >> 16; x2 = (-7357 * p) >> 16;
Press = p + ((x1 + x2 + 3791) >> 4); Press = p + ((x1 + x2 + 3791) >> 4);
} }

94
libraries/AP_Baro/AP_Baro_BMP085.h
View File

@ -1,49 +1,49 @@
#ifndef APM_BMP085_h #ifndef APM_BMP085_h
#define APM_BMP085_h #define APM_BMP085_h
#define TEMP_FILTER_SIZE 4 #define TEMP_FILTER_SIZE 4
#define PRESS_FILTER_SIZE 8 #define PRESS_FILTER_SIZE 8
#include "APM_BMP085_hil.h" #include "APM_BMP085_hil.h"
class APM_BMP085_Class class APM_BMP085_Class
{ {
public: public:
APM_BMP085_Class(): APM_BMP085_Class():
_temp_index(0), _temp_index(0),
_press_index(0){}; // Constructor _press_index(0){}; // Constructor
int32_t RawPress; int32_t RawPress;
int32_t _offset_press; int32_t _offset_press;
int32_t RawTemp; int32_t RawTemp;
int16_t Temp; int16_t Temp;
int32_t Press; int32_t Press;
//int Altitude; //int Altitude;
uint8_t oss; uint8_t oss;
bool _apm2_hardware; bool _apm2_hardware;
//int32_t Press0; // Pressure at sea level //int32_t Press0; // Pressure at sea level
bool Init(int initialiseWireLib = 1, bool apm2_hardware=false); bool Init(int initialiseWireLib = 1, bool apm2_hardware=false);
uint8_t Read(); uint8_t Read();
private: private:
// State machine // State machine
uint8_t BMP085_State; uint8_t BMP085_State;
// Internal calibration registers // Internal calibration registers
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;
int _temp_filter[TEMP_FILTER_SIZE]; int _temp_filter[TEMP_FILTER_SIZE];
int _press_filter[PRESS_FILTER_SIZE]; int _press_filter[PRESS_FILTER_SIZE];
long _offset_temp; long _offset_temp;
uint8_t _temp_index; uint8_t _temp_index;
uint8_t _press_index; uint8_t _press_index;
void Command_ReadPress(); void Command_ReadPress();
void Command_ReadTemp(); void Command_ReadTemp();
void ReadPress(); void ReadPress();
void ReadTemp(); void ReadTemp();
void Calculate(); void Calculate();
}; };
#endif #endif