diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index df744b7117..f410614373 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -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 dev) +AP_Baro_BMP085::AP_Baro_BMP085(AP_Baro &baro, AP_HAL::OwnPtr dev) : AP_Baro_Backend(baro) , _dev(std::move(dev)) +{ } + +AP_Baro_Backend * AP_Baro_BMP085::probe(AP_Baro &baro, AP_HAL::OwnPtr 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 _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(®, 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 diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h index 7bf345cdec..3003eec5c4 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.h +++ b/libraries/AP_Baro/AP_Baro_BMP085.h @@ -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 dev); + AP_Baro_BMP085(AP_Baro &baro, AP_HAL::OwnPtr dev); /* AP_Baro public interface: */ void update(); + + static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr 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 _dev; + uint16_t _read_prom_word(uint8_t word); + bool _read_prom(uint16_t *prom); + + + AP_HAL::OwnPtr _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 _pressure_filter; + + uint8_t _vers; + uint8_t _type; };