diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index f7c2593079..95fc329e04 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -27,6 +27,7 @@ #include #include "AP_Baro_BMP085.h" +#include "AP_Baro_BMP280.h" #include "AP_Baro_HIL.h" #include "AP_Baro_MS5611.h" #include "AP_Baro_PX4.h" @@ -362,6 +363,12 @@ void AP_Baro::init(void) drivers[0] = new AP_Baro_BMP085(*this, std::move(hal.i2c_mgr->get_device(HAL_BARO_BMP085_BUS, HAL_BARO_BMP085_I2C_ADDR))); _num_drivers = 1; +#elif HAL_BARO_DEFAULT == HAL_BARO_BMP280_I2C + ADD_BACKEND(AP_Baro_BMP280::probe(*this, + std::move(hal.i2c_mgr->get_device(HAL_BARO_BMP280_BUS, HAL_BARO_BMP280_I2C_ADDR)))); +#elif HAL_BARO_DEFAULT == HAL_BARO_BMP280_SPI + ADD_BACKEND(AP_Baro_BMP280::probe(*this, + std::move(hal.spi->get_device(HAL_BARO_BMP280_NAME)))); #elif HAL_BARO_DEFAULT == HAL_BARO_MS5611_I2C ADD_BACKEND(AP_Baro_MS56XX::probe(*this, std::move(hal.i2c_mgr->get_device(HAL_BARO_MS5611_I2C_BUS, HAL_BARO_MS5611_I2C_ADDR)))); diff --git a/libraries/AP_Baro/AP_Baro_BMP280.cpp b/libraries/AP_Baro/AP_Baro_BMP280.cpp new file mode 100644 index 0000000000..b9da66dae3 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_BMP280.cpp @@ -0,0 +1,197 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +#include "AP_Baro_BMP280.h" + +#include + +extern const AP_HAL::HAL &hal; + +#define BMP280_MODE_SLEEP 0 +#define BMP280_MODE_FORCED 1 +#define BMP280_MODE_NORMAL 3 +#define BMP280_MODE BMP280_MODE_NORMAL + +#define BMP280_OVERSAMPLING_1 1 +#define BMP280_OVERSAMPLING_2 2 +#define BMP280_OVERSAMPLING_4 3 +#define BMP280_OVERSAMPLING_8 4 +#define BMP280_OVERSAMPLING_16 5 +#define BMP280_OVERSAMPLING_P BMP280_OVERSAMPLING_16 +#define BMP280_OVERSAMPLING_T BMP280_OVERSAMPLING_2 + +#define BMP280_FILTER_COEFFICIENT 2 + +#define BMP280_ID 0x58 + +#define BMP280_REG_CALIB 0x88 +#define BMP280_REG_ID 0xD0 +#define BMP280_REG_RESET 0xE0 +#define BMP280_REG_STATUS 0xF3 +#define BMP280_REG_CTRL_MEAS 0xF4 +#define BMP280_REG_CONFIG 0xF5 +#define BMP280_REG_DATA 0xF7 + +AP_Baro_BMP280::AP_Baro_BMP280(AP_Baro &baro, AP_HAL::OwnPtr dev) + : AP_Baro_Backend(baro) + , _dev(std::move(dev)) +{ +} + +AP_Baro_Backend *AP_Baro_BMP280::probe(AP_Baro &baro, + AP_HAL::OwnPtr dev) +{ + if (!dev) { + return nullptr; + } + + AP_Baro_BMP280 *sensor = new AP_Baro_BMP280(baro, std::move(dev)); + if (!sensor || !sensor->_init()) { + delete sensor; + return nullptr; + } + return sensor; +} + +bool AP_Baro_BMP280::_init() +{ + if (!_dev | !_dev->get_semaphore()->take(0)) { + return false; + } + + _has_sample = false; + + _dev->set_speed(AP_HAL::Device::SPEED_HIGH); + + uint8_t whoami; + if (!_dev->read_registers(BMP280_REG_ID, &whoami, 1) || + whoami != BMP280_ID) { + // not a BMP280 + _dev->get_semaphore()->give(); + return false; + } + + // read the calibration data + uint8_t buf[24]; + _dev->read_registers(BMP280_REG_CALIB, buf, sizeof(buf)); + + _t1 = ((int16_t)buf[1] << 8) | buf[0]; + _t2 = ((int16_t)buf[3] << 8) | buf[2]; + _t3 = ((int16_t)buf[5] << 8) | buf[4]; + _p1 = ((int16_t)buf[7] << 8) | buf[6]; + _p2 = ((int16_t)buf[9] << 8) | buf[8]; + _p3 = ((int16_t)buf[11] << 8) | buf[10]; + _p4 = ((int16_t)buf[13] << 8) | buf[12]; + _p5 = ((int16_t)buf[15] << 8) | buf[14]; + _p6 = ((int16_t)buf[17] << 8) | buf[16]; + _p7 = ((int16_t)buf[19] << 8) | buf[18]; + _p8 = ((int16_t)buf[21] << 8) | buf[20]; + _p9 = ((int16_t)buf[23] << 8) | buf[22]; + + // SPI write needs bit mask + uint8_t mask = 0xFF; + if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) { + mask = 0x7F; + } + + _dev->write_register((BMP280_REG_CTRL_MEAS & mask), (BMP280_OVERSAMPLING_T << 5) | + (BMP280_OVERSAMPLING_P << 2) | BMP280_MODE); + + _dev->write_register((BMP280_REG_CONFIG & mask), BMP280_FILTER_COEFFICIENT << 2); + + _instance = _frontend.register_sensor(); + + _dev->get_semaphore()->give(); + + // request 50Hz update + _dev->register_periodic_callback(20 * USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP280::_timer, bool)); + + return true; +} + + + +// acumulate a new sensor reading +bool AP_Baro_BMP280::_timer(void) +{ + uint8_t buf[6]; + + _dev->read_registers(BMP280_REG_DATA, buf, sizeof(buf)); + + _update_temperature((buf[3] << 12) | (buf[4] << 4) | (buf[5] >> 4)); + _update_pressure((buf[0] << 12) | (buf[1] << 4) | (buf[2] >> 4)); + + return true; +} + +// transfer data to the frontend +void AP_Baro_BMP280::update(void) +{ + if (_sem->take_nonblocking()) { + if (!_has_sample) { + _sem->give(); + return; + } + + _copy_to_frontend(_instance, _pressure, _temperature); + _has_sample = false; + _sem->give(); + } +} + +// calculate temperature +void AP_Baro_BMP280::_update_temperature(int32_t temp_raw) +{ + int32_t var1, var2, t; + + // according to datasheet page 22 + var1 = ((((temp_raw >> 3) - ((int32_t)_t1 << 1))) * ((int32_t)_t2)) >> 11; + var2 = (((((temp_raw >> 4) - ((int32_t)_t1)) * ((temp_raw >> 4) - ((int32_t)_t1))) >> 12) * ((int32_t)_t3)) >> 14; + _t_fine = var1 + var2; + t = (_t_fine * 5 + 128) >> 8; + if (_sem->take(0)) { + _temperature = ((float)t) / 100; + _sem->give(); + } +} + +// calculate pressure +void AP_Baro_BMP280::_update_pressure(int32_t press_raw) +{ + int64_t var1, var2, p; + + // according to datasheet page 22 + var1 = ((int64_t)_t_fine) - 128000; + var2 = var1 * var1 * (int64_t)_p6; + var2 = var2 + ((var1 * (int64_t)_p5) << 17); + var2 = var2 + (((int64_t)_p4) << 35); + var1 = ((var1 * var1 * (int64_t)_p3) >> 8) + ((var1 * (int64_t)_p2) << 12); + var1 = (((((int64_t)1) << 47) + var1)) * ((int64_t)_p1) >> 33; + + if (var1 == 0) { + return; + } + + p = 1048576 - press_raw; + p = (((p << 31) - var2) * 3125) / var1; + var1 = (((int64_t)_p9) * (p >> 13) * (p >> 13)) >> 25; + var2 = (((int64_t)_p8) * p) >> 19; + p = ((p + var1 + var2) >> 8) + (((int64_t)_p7) << 4); + + if (_sem->take(0)) { + _pressure = (float)p / 25600; + _has_sample = true; + _sem->give(); + } +} diff --git a/libraries/AP_Baro/AP_Baro_BMP280.h b/libraries/AP_Baro/AP_Baro_BMP280.h new file mode 100644 index 0000000000..bab6707929 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_BMP280.h @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include + +#include "AP_Baro_Backend.h" + +class AP_Baro_BMP280 : public AP_Baro_Backend +{ +public: + AP_Baro_BMP280(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: + virtual ~AP_Baro_BMP280(void) {}; + + bool _init(void); + bool _timer(void); + void _update_temperature(int32_t); + void _update_pressure(int32_t); + + AP_HAL::OwnPtr _dev; + + bool _has_sample; + uint8_t _instance; + int32_t _t_fine; + float _pressure; + float _temperature; + + // Internal calibration registers + int16_t _t2, _t3, _p2, _p3, _p4, _p5, _p6, _p7, _p8, _p9; + uint16_t _t1, _p1; +};