Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: fixed BMP085 driver for current API

Browse Source
This commit is contained in:
night-ghost 2018-01-22 14:31:01 +05:00 committed by Andrew Tridgell
parent bf758098d7
commit 2c043b822b
2 changed files with 136 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

144
libraries/AP_Baro/AP_Baro_BMP085.cpp
View File

@ -33,11 +33,33 @@ extern const AP_HAL::HAL &hal;
#define OVERSAMPLING BMP085_OVERSAMPLING_HIGHRES
#endif
AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev)
: AP_Baro_Backend(baro)
, _dev(std::move(dev))
{ }
AP_Baro_Backend * AP_Baro_BMP085::probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev)
{
uint8_t buff[22];
if (!dev) {
return nullptr;
}
AP_Baro_BMP085 *sensor = new AP_Baro_BMP085(baro, std::move(dev));
if (!sensor || !sensor->_init()) {
delete sensor;
return nullptr;
}
return sensor;
}
bool AP_Baro_BMP085::_init()
{
union {
uint8_t buff[22];
uint16_t wb[11];
} bb;
// get pointer to i2c bus semaphore
AP_HAL::Semaphore *sem = _dev->get_semaphore();
@ -52,36 +74,99 @@ AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::I2CDevice>
_eoc->mode(HAL_GPIO_INPUT);
}
uint8_t id;
if (!_dev->read_registers(0xD0, &id, 1)) {
sem->give();
return false;
}
if(id!=0x55) return false; // not BMP180
_dev->read_registers(0xD1, &_vers, 1);
bool prom_ok=false;
_type=0;
// We read the calibration data registers
if (!_dev->read_registers(0xAA, buff, sizeof(buff))) {
AP_HAL::panic("BMP085: bad calibration registers");
if (_dev->read_registers(0xAA, bb.buff, sizeof(bb.buff))) {
prom_ok=true;
}
ac1 = ((int16_t)buff[0] << 8) | buff[1];
ac2 = ((int16_t)buff[2] << 8) | buff[3];
ac3 = ((int16_t)buff[4] << 8) | buff[5];
ac4 = ((int16_t)buff[6] << 8) | buff[7];
ac5 = ((int16_t)buff[8] << 8) | buff[9];
ac6 = ((int16_t)buff[10] << 8) | buff[11];
b1 = ((int16_t)buff[12] << 8) | buff[13];
b2 = ((int16_t)buff[14] << 8) | buff[15];
mb = ((int16_t)buff[16] << 8) | buff[17];
mc = ((int16_t)buff[18] << 8) | buff[19];
md = ((int16_t)buff[20] << 8) | buff[21];
if(!prom_ok){
if(_read_prom((uint16_t *)&bb.wb[0])) { // BMP180 requires reads by 2 bytes
prom_ok=true;
_type=1;
}
}
if(!prom_ok){
sem->give();
return false;
}
ac1 = ((int16_t)bb.buff[0] << 8) | bb.buff[1];
ac2 = ((int16_t)bb.buff[2] << 8) | bb.buff[3];
ac3 = ((int16_t)bb.buff[4] << 8) | bb.buff[5];
ac4 = ((int16_t)bb.buff[6] << 8) | bb.buff[7];
ac5 = ((int16_t)bb.buff[8] << 8) | bb.buff[9];
ac6 = ((int16_t)bb.buff[10]<< 8) | bb.buff[11];
b1 = ((int16_t)bb.buff[12] << 8) | bb.buff[13];
b2 = ((int16_t)bb.buff[14] << 8) | bb.buff[15];
mb = ((int16_t)bb.buff[16] << 8) | bb.buff[17];
mc = ((int16_t)bb.buff[18] << 8) | bb.buff[19];
md = ((int16_t)bb.buff[20] << 8) | bb.buff[21];
if( (ac1==0 || ac1==-1) ||
(ac2==0 || ac2==-1) ||
(ac3==0 || ac3==-1) ||
(ac4==0 || ac4==0xFFFF) ||
(ac5==0 || ac5==0xFFFF) ||
(ac6==0 || ac6==0xFFFF) ) return false;
_last_press_read_command_time = 0;
_last_temp_read_command_time = 0;
_instance = _frontend.register_sensor();
// Send a command to read temperature
_cmd_read_temp();
_state = 0;
_instance = _frontend.register_sensor();
sem->give();
_dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP085::_timer, void));
return true;
}
uint16_t AP_Baro_BMP085::_read_prom_word(uint8_t word)
{
const uint8_t reg = 0xAA + (word << 1);
uint8_t val[2];
if (!_dev->transfer(&reg, 1, val, sizeof(val))) {
return 0;
}
return (val[0] << 8) | val[1];
}
bool AP_Baro_BMP085::_read_prom(uint16_t *prom)
{
bool all_zero = true;
for (uint8_t i = 0; i < 11; i++) {
prom[i] = _read_prom_word(i);
if (prom[i] != 0) {
all_zero = false;
}
}
if (all_zero) {
return false;
}
return true;
}
/*
@ -138,17 +223,24 @@ void AP_Baro_BMP085::_cmd_read_pressure()
bool AP_Baro_BMP085::_read_pressure()
{
uint8_t buf[3];
if (!_dev->read_registers(0xF6, buf, sizeof(buf))) {
_retry_time = AP_HAL::millis() + 1000;
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
return false;
if (_dev->read_registers(0xF6, buf, sizeof(buf))) {
_raw_pressure = (((uint32_t)buf[0] << 16)
| ((uint32_t)buf[1] << 8)
| ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
return true;
}
_raw_pressure = (((uint32_t)buf[0] << 16)
| ((uint32_t)buf[1] << 8)
| ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
return true;
uint8_t xlsb;
if (_dev->read_registers(0xF6, buf, 2) && _dev->read_registers(0xF8, &xlsb, 1) ) {
_raw_pressure = (((uint32_t)buf[0] << 16)
| ((uint32_t)buf[1] << 8)
| ((uint32_t)xlsb)) >> (8 - OVERSAMPLING);
return true;
}
_last_press_read_command_time = AP_HAL::millis() + 1000;
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
return false;
}
// Send Command to Read Temperature

21
libraries/AP_Baro/AP_Baro_BMP085.h
View File

@ -7,15 +7,24 @@
#include "AP_Baro_Backend.h"
#ifndef HAL_BARO_BMP085_I2C_ADDR
#define HAL_BARO_BMP085_I2C_ADDR (0x77)
#endif
class AP_Baro_BMP085 : public AP_Baro_Backend
{
public:
AP_Baro_BMP085(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev);
AP_Baro_BMP085(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
/* AP_Baro public interface: */
void update();
static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
private:
bool _init();
void _cmd_read_pressure();
void _cmd_read_temp();
bool _read_pressure();
@ -25,7 +34,11 @@ private:
void _timer(void);
AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev;
uint16_t _read_prom_word(uint8_t word);
bool _read_prom(uint16_t *prom);
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
AP_HAL::DigitalSource *_eoc;
uint8_t _instance;
@ -42,9 +55,11 @@ private:
int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
uint16_t ac4, ac5, ac6;
uint32_t _retry_time;
int32_t _raw_pressure;
int32_t _raw_temp;
int32_t _temp;
AverageIntegralFilter<int32_t, int32_t, 10> _pressure_filter;
uint8_t _vers;
uint8_t _type;
};