mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-07 00:13:59 -04:00
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:
parent
81a298c9c8
commit
c5e97129c1
@ -13,12 +13,6 @@
|
|||||||
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>
|
||||||
@ -26,7 +20,8 @@
|
|||||||
|
|
||||||
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,16 +77,17 @@ 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
|
||||||
@ -113,28 +98,27 @@ void AP_Baro_BMP085::accumulate(void)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// 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,7 +142,7 @@ 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,
|
||||||
@ -166,8 +150,8 @@ void AP_Baro_BMP085::Command_ReadPress()
|
|||||||
_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);
|
||||||
|
@ -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();
|
|
||||||
};
|
};
|
||||||
|
Loading…
Reference in New Issue
Block a user