AP_Baro: BMP085: follow coding style

- Spacing changes and variable renames to follow coding style
 - No need to initialize variables to 0, it's already done by our
   global new operator.
This commit is contained in:
Lucas De Marchi 2015-11-23 12:11:40 -02:00 committed by Andrew Tridgell
parent 81a298c9c8
commit c5e97129c1
2 changed files with 54 additions and 71 deletions

View File

@ -13,20 +13,15 @@
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
/*
BMP085 barometer driver. Based on original code by Jordi Munoz and
Jose Julio
Substantially modified by Andrew Tridgell
*/
#include "AP_Baro_BMP085.h" #include "AP_Baro_BMP085.h"
#include <AP_Common/AP_Common.h> #include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL &hal;
#define BMP085_ADDRESS 0x77 //(0xEE >> 1) // 0xEE >> 1
#define BMP085_ADDRESS 0x77
#ifndef BMP085_EOC #ifndef BMP085_EOC
// No EOC connection from Baro // No EOC connection from Baro
@ -42,19 +37,8 @@ extern const AP_HAL::HAL& hal;
// oversampling 3 gives 26ms conversion time. We then average // oversampling 3 gives 26ms conversion time. We then average
#define OVERSAMPLING 3 #define OVERSAMPLING 3
/* AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro)
constructor : AP_Baro_Backend(baro)
*/
AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro) :
AP_Baro_Backend(baro),
_instance(0),
_temp_sum(0),
_press_sum(0),
_count(0),
BMP085_State(0),
ac1(0), ac2(0), ac3(0), b1(0), b2(0), mb(0), mc(0), md(0),
ac4(0), ac5(0), ac6(0),
_retry_time(0)
{ {
uint8_t buff[22]; uint8_t buff[22];
@ -93,48 +77,48 @@ AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro) :
_instance = _frontend.register_sensor(); _instance = _frontend.register_sensor();
//Send a command to read Temp // Send a command to read temperature
Command_ReadTemp(); _cmd_read_temp();
BMP085_State = 0; BMP085_State = 0;
i2c_sem->give(); i2c_sem->give();
} }
// Read the sensor. This is a state machine /*
// acumulate a new sensor reading This is a state machine. Acumulate a new sensor reading.
*/
void AP_Baro_BMP085::accumulate(void) void AP_Baro_BMP085::accumulate(void)
{ {
// get pointer to i2c bus semaphore // get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore(); AP_HAL::Semaphore *i2c_sem = hal.i2c->get_semaphore();
if (!BMP_DATA_READY()) { if (!BMP_DATA_READY()) {
return; return;
} }
// take i2c bus sempahore // take i2c bus sempahore
if (!i2c_sem->take(1)) if (!i2c_sem->take(1)) {
return; return;
}
if (BMP085_State == 0) { if (BMP085_State == 0) {
ReadTemp(); _read_temp();
} else { } else if (_read_pressure()) {
if (ReadPress()) { _calculate();
Calculate();
}
} }
BMP085_State++; BMP085_State++;
if (BMP085_State == 5) { if (BMP085_State == 5) {
BMP085_State = 0; BMP085_State = 0;
Command_ReadTemp(); _cmd_read_temp();
} else { } else {
Command_ReadPress(); _cmd_read_pressure();
} }
i2c_sem->give(); i2c_sem->give();
} }
/* /*
transfer data to the frontend transfer data to the frontend
*/ */
@ -158,16 +142,16 @@ void AP_Baro_BMP085::update(void)
} }
// Send command to Read Pressure // Send command to Read Pressure
void AP_Baro_BMP085::Command_ReadPress() void AP_Baro_BMP085::_cmd_read_pressure()
{ {
// Mode 0x34+(OVERSAMPLING << 6) is osrs=3 when OVERSAMPLING=3 => 25.5ms conversion time // Mode 0x34+(OVERSAMPLING << 6) is osrs=3 when OVERSAMPLING=3 => 25.5ms conversion time
hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4, hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4,
0x34+(OVERSAMPLING << 6)); 0x34 + (OVERSAMPLING << 6));
_last_press_read_command_time = AP_HAL::millis(); _last_press_read_command_time = AP_HAL::millis();
} }
// Read Raw Pressure values // Read raw pressure values
bool AP_Baro_BMP085::ReadPress() bool AP_Baro_BMP085::_read_pressure()
{ {
uint8_t buf[3]; uint8_t buf[3];
@ -177,21 +161,21 @@ bool AP_Baro_BMP085::ReadPress()
return false; return false;
} }
RawPress = (((uint32_t)buf[0] << 16) _raw_pressure = (((uint32_t)buf[0] << 16)
| ((uint32_t)buf[1] << 8) | ((uint32_t)buf[1] << 8)
| ((uint32_t)buf[2])) >> (8 - OVERSAMPLING); | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
return true; return true;
} }
// Send Command to Read Temperature // Send Command to Read Temperature
void AP_Baro_BMP085::Command_ReadTemp() void AP_Baro_BMP085::_cmd_read_temp()
{ {
hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4, 0x2E); hal.i2c->writeRegister(BMP085_ADDRESS, 0xF4, 0x2E);
_last_temp_read_command_time = AP_HAL::millis(); _last_temp_read_command_time = AP_HAL::millis();
} }
// Read Raw Temperature values // Read raw temperature values
void AP_Baro_BMP085::ReadTemp() void AP_Baro_BMP085::_read_temp()
{ {
uint8_t buf[2]; uint8_t buf[2];
int32_t _temp_sensor; int32_t _temp_sensor;
@ -203,12 +187,11 @@ 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];
RawTemp = _temp_sensor; _raw_temp = _temp_sensor;
} }
// _calculate Temperature and Pressure in real units.
// Calculate Temperature and Pressure in real units. void AP_Baro_BMP085::_calculate()
void AP_Baro_BMP085::Calculate()
{ {
int32_t x1, x2, x3, b3, b5, b6, p; int32_t x1, x2, x3, b3, b5, b6, p;
uint32_t b4, b7; uint32_t b4, b7;
@ -217,7 +200,7 @@ void AP_Baro_BMP085::Calculate()
// 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 = ((int32_t)RawTemp - ac6) * ac5 >> 15; x1 = ((int32_t)_raw_temp - ac6) * ac5 >> 15;
x2 = ((int32_t) mc << 11) / (x1 + md); x2 = ((int32_t) mc << 11) / (x1 + md);
b5 = x1 + x2; b5 = x1 + x2;
_temp_sum += (b5 + 8) >> 4; _temp_sum += (b5 + 8) >> 4;
@ -236,7 +219,7 @@ void AP_Baro_BMP085::Calculate()
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 >> OVERSAMPLING); b7 = ((uint32_t) _raw_pressure - b3) * (50000 >> OVERSAMPLING);
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);

View File

@ -14,12 +14,18 @@ public:
void accumulate(void); void accumulate(void);
private: private:
void _cmd_read_pressure();
void _cmd_read_temp();
bool _read_pressure();
void _read_temp();
void _calculate();
uint8_t _instance; uint8_t _instance;
float _temp_sum; float _temp_sum;
float _press_sum; float _press_sum;
uint8_t _count; uint8_t _count;
// Flymaple has no EOC pin, so use times instead // Boards with no EOC pin: use times instead
uint32_t _last_press_read_command_time; uint32_t _last_press_read_command_time;
uint32_t _last_temp_read_command_time; uint32_t _last_temp_read_command_time;
@ -31,12 +37,6 @@ private:
uint16_t ac4, ac5, ac6; uint16_t ac4, ac5, ac6;
uint32_t _retry_time; uint32_t _retry_time;
int32_t RawPress; int32_t _raw_pressure;
int32_t RawTemp; int32_t _raw_temp;
void Command_ReadPress();
void Command_ReadTemp();
bool ReadPress();
void ReadTemp();
void Calculate();
}; };