Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : #include "AP_Baro_BMP280.h"
      16             : 
      17             : #if AP_BARO_BMP280_ENABLED
      18             : 
      19             : #include <AP_Math/definitions.h>
      20             : 
      21             : extern const AP_HAL::HAL &hal;
      22             : 
      23             : #define BMP280_MODE_SLEEP  0
      24             : #define BMP280_MODE_FORCED 1
      25             : #define BMP280_MODE_NORMAL 3
      26             : #define BMP280_MODE BMP280_MODE_NORMAL
      27             : 
      28             : #define BMP280_OVERSAMPLING_1  1
      29             : #define BMP280_OVERSAMPLING_2  2
      30             : #define BMP280_OVERSAMPLING_4  3
      31             : #define BMP280_OVERSAMPLING_8  4
      32             : #define BMP280_OVERSAMPLING_16 5
      33             : #define BMP280_OVERSAMPLING_P BMP280_OVERSAMPLING_16
      34             : #define BMP280_OVERSAMPLING_T BMP280_OVERSAMPLING_2
      35             : 
      36             : #define BMP280_FILTER_COEFFICIENT 2
      37             : 
      38             : #define BMP280_ID            0x58
      39             : #define BME280_ID            0x60
      40             : 
      41             : #define BMP280_REG_CALIB     0x88
      42             : #define BMP280_REG_ID        0xD0
      43             : #define BMP280_REG_RESET     0xE0
      44             : #define BMP280_REG_STATUS    0xF3
      45             : #define BMP280_REG_CTRL_MEAS 0xF4
      46             : #define BMP280_REG_CONFIG    0xF5
      47             : #define BMP280_REG_DATA      0xF7
      48             : 
      49           0 : AP_Baro_BMP280::AP_Baro_BMP280(AP_Baro &baro, AP_HAL::Device &dev)
      50             :     : AP_Baro_Backend(baro)
      51           0 :     , _dev(&dev)
      52             : {
      53           0 : }
      54             : 
      55           0 : AP_Baro_Backend *AP_Baro_BMP280::probe(AP_Baro &baro, AP_HAL::Device &dev)
      56             : {
      57           0 :     AP_Baro_BMP280 *sensor = NEW_NOTHROW AP_Baro_BMP280(baro, dev);
      58           0 :     if (!sensor || !sensor->_init()) {
      59           0 :         delete sensor;
      60           0 :         return nullptr;
      61             :     }
      62           0 :     return sensor;
      63             : }
      64             : 
      65           0 : bool AP_Baro_BMP280::_init()
      66             : {
      67           0 :     if (!_dev) {
      68           0 :         return false;
      69             :     }
      70           0 :     WITH_SEMAPHORE(_dev->get_semaphore());
      71             : 
      72           0 :     _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
      73             : 
      74             :     uint8_t whoami;
      75           0 :     if (!_dev->read_registers(BMP280_REG_ID, &whoami, 1)  ||
      76           0 :         (whoami != BME280_ID && whoami != BMP280_ID)) {
      77             :         // not a BMP280 or BME280
      78           0 :         return false;
      79             :     }
      80             : 
      81             :     // read the calibration data
      82             :     uint8_t buf[24];
      83           0 :     if (!_dev->read_registers(BMP280_REG_CALIB, buf, sizeof(buf))) {
      84           0 :         return false;
      85             :     }
      86             : 
      87           0 :     _t1 = ((int16_t)buf[1] << 8) | buf[0];
      88           0 :     _t2 = ((int16_t)buf[3] << 8) | buf[2];
      89           0 :     _t3 = ((int16_t)buf[5] << 8) | buf[4];
      90           0 :     _p1 = ((int16_t)buf[7] << 8) | buf[6];
      91           0 :     _p2 = ((int16_t)buf[9] << 8) | buf[8];
      92           0 :     _p3 = ((int16_t)buf[11] << 8) | buf[10];
      93           0 :     _p4 = ((int16_t)buf[13] << 8) | buf[12];
      94           0 :     _p5 = ((int16_t)buf[15] << 8) | buf[14];
      95           0 :     _p6 = ((int16_t)buf[17] << 8) | buf[16];
      96           0 :     _p7 = ((int16_t)buf[19] << 8) | buf[18];
      97           0 :     _p8 = ((int16_t)buf[21] << 8) | buf[20];
      98           0 :     _p9 = ((int16_t)buf[23] << 8) | buf[22];
      99             : 
     100             :     // SPI write needs bit mask
     101           0 :     uint8_t mask = 0xFF;
     102           0 :     if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
     103           0 :         mask = 0x7F;
     104             :     }
     105             : 
     106           0 :     _dev->setup_checked_registers(2, 20);
     107             :     
     108           0 :     _dev->write_register((BMP280_REG_CTRL_MEAS & mask), (BMP280_OVERSAMPLING_T << 5) |
     109             :                          (BMP280_OVERSAMPLING_P << 2) | BMP280_MODE, true);
     110             : 
     111           0 :     _dev->write_register((BMP280_REG_CONFIG & mask), BMP280_FILTER_COEFFICIENT << 2, true);
     112             : 
     113           0 :     _instance = _frontend.register_sensor();
     114             : 
     115           0 :     _dev->set_device_type(DEVTYPE_BARO_BMP280);
     116           0 :     set_bus_id(_instance, _dev->get_bus_id());
     117             :     
     118             :     // request 50Hz update
     119           0 :     _dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP280::_timer, void));
     120             : 
     121           0 :     return true;
     122           0 : }
     123             : 
     124             : 
     125             : 
     126             : //  accumulate a new sensor reading
     127           0 : void AP_Baro_BMP280::_timer(void)
     128             : {
     129             :     uint8_t buf[6];
     130             : 
     131           0 :     if (_dev->read_registers(BMP280_REG_DATA, buf, sizeof(buf))) {
     132           0 :         _update_temperature((buf[3] << 12) | (buf[4] << 4) | (buf[5] >> 4));
     133           0 :         _update_pressure((buf[0] << 12) | (buf[1] << 4) | (buf[2] >> 4));
     134             :     }
     135             : 
     136           0 :     _dev->check_next_register();
     137           0 : }
     138             : 
     139             : // transfer data to the frontend
     140           0 : void AP_Baro_BMP280::update(void)
     141             : {
     142           0 :     WITH_SEMAPHORE(_sem);
     143             : 
     144           0 :     if (_pressure_count == 0) {
     145           0 :         return;
     146             :     }
     147             : 
     148           0 :     _copy_to_frontend(_instance, _pressure_sum/_pressure_count, _temperature);
     149           0 :     _pressure_count = 0;
     150           0 :     _pressure_sum = 0;
     151           0 : }
     152             : 
     153             : // calculate temperature
     154           0 : void AP_Baro_BMP280::_update_temperature(int32_t temp_raw)
     155             : {
     156             :     int32_t var1, var2, t;
     157             : 
     158             :     // according to datasheet page 22
     159           0 :     var1 = ((((temp_raw >> 3) - ((int32_t)_t1 << 1))) * ((int32_t)_t2)) >> 11;
     160           0 :     var2 = (((((temp_raw >> 4) - ((int32_t)_t1)) * ((temp_raw >> 4) - ((int32_t)_t1))) >> 12) * ((int32_t)_t3)) >> 14;
     161           0 :     _t_fine = var1 + var2;
     162           0 :     t = (_t_fine * 5 + 128) >> 8;
     163             : 
     164           0 :     const float temp = ((float)t) * 0.01f;
     165             : 
     166           0 :     WITH_SEMAPHORE(_sem);
     167             :     
     168           0 :     _temperature = temp;
     169           0 : }
     170             : 
     171             : // calculate pressure
     172           0 : void AP_Baro_BMP280::_update_pressure(int32_t press_raw)
     173             : {
     174             :     int64_t var1, var2, p;
     175             : 
     176             :     // according to datasheet page 22
     177           0 :     var1 = ((int64_t)_t_fine) - 128000;
     178           0 :     var2 = var1 * var1 * (int64_t)_p6;
     179           0 :     var2 = var2 + ((var1 * (int64_t)_p5) << 17);
     180           0 :     var2 = var2 + (((int64_t)_p4) << 35);
     181           0 :     var1 = ((var1 * var1 * (int64_t)_p3) >> 8) + ((var1 * (int64_t)_p2) << 12);
     182           0 :     var1 = (((((int64_t)1) << 47) + var1)) * ((int64_t)_p1) >> 33;
     183             : 
     184           0 :     if (var1 == 0) {
     185           0 :         return;
     186             :     }
     187             : 
     188           0 :     p = 1048576 - press_raw;
     189           0 :     p = (((p << 31) - var2) * 3125) / var1;
     190           0 :     var1 = (((int64_t)_p9) * (p >> 13) * (p >> 13)) >> 25;
     191           0 :     var2 = (((int64_t)_p8) * p) >> 19;
     192           0 :     p = ((p + var1 + var2) >> 8) + (((int64_t)_p7) << 4);
     193             : 
     194             : 
     195           0 :     const float press = (float)p / 256.0f;
     196           0 :     if (!pressure_ok(press)) {
     197           0 :         return;
     198             :     }
     199             :     
     200           0 :     WITH_SEMAPHORE(_sem);
     201             :     
     202           0 :     _pressure_sum += press;
     203           0 :     _pressure_count++;
     204           0 : }
     205             : 
     206             : #endif  // AP_BARO_BMP280_ENABLED