diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index d078079e20..d2492599e3 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -33,6 +33,7 @@ #include "AP_Baro_SITL.h" #include "AP_Baro_BMP085.h" #include "AP_Baro_BMP280.h" +#include "AP_Baro_SPL06.h" #include "AP_Baro_HIL.h" #include "AP_Baro_KellerLD.h" #include "AP_Baro_MS5611.h" diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index fd3cda7e01..47a767b216 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -215,6 +215,7 @@ private: PROBE_KELLER=(1<<8), PROBE_MS5837=(1<<9), PROBE_BMP388=(1<<10), + PROBE_SPL06=(1<<11), }; struct sensor { diff --git a/libraries/AP_Baro/AP_Baro_BMP280.cpp b/libraries/AP_Baro/AP_Baro_BMP280.cpp index f2831bdbb8..f668e12e41 100644 --- a/libraries/AP_Baro/AP_Baro_BMP280.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP280.cpp @@ -34,6 +34,7 @@ extern const AP_HAL::HAL &hal; #define BMP280_FILTER_COEFFICIENT 2 #define BMP280_ID 0x58 +#define BME280_ID 0x60 #define BMP280_REG_CALIB 0x88 #define BMP280_REG_ID 0xD0 @@ -77,8 +78,8 @@ bool AP_Baro_BMP280::_init() uint8_t whoami; if (!_dev->read_registers(BMP280_REG_ID, &whoami, 1) || - whoami != BMP280_ID) { - // not a BMP280 + (whoami != BME280_ID && whoami != BMP280_ID)) { + // not a BMP280 or BME280 return false; } diff --git a/libraries/AP_Baro/AP_Baro_BMP388.cpp b/libraries/AP_Baro/AP_Baro_BMP388.cpp index b32ab9329c..ab2315bf0f 100644 --- a/libraries/AP_Baro/AP_Baro_BMP388.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP388.cpp @@ -89,15 +89,15 @@ bool AP_Baro_BMP388::init() dev->write_register(BMP388_REG_PWR_CTRL, 0x33, true); uint8_t whoami; - if (!dev->read_registers(BMP388_REG_ID, &whoami, 1) || + if (!read_registers(BMP388_REG_ID, &whoami, 1) || whoami != BMP388_ID) { // not a BMP388 return false; } // read the calibration data - dev->read_registers(BMP388_REG_CAL_P, (uint8_t *)&calib_p, sizeof(calib_p)); - dev->read_registers(BMP388_REG_CAL_T, (uint8_t *)&calib_t, sizeof(calib_t)); + read_registers(BMP388_REG_CAL_P, (uint8_t *)&calib_p, sizeof(calib_p)); + read_registers(BMP388_REG_CAL_T, (uint8_t *)&calib_t, sizeof(calib_t)); scale_calibration_data(); @@ -121,7 +121,7 @@ void AP_Baro_BMP388::timer(void) { uint8_t buf[7]; - if (!dev->read_registers(BMP388_REG_STATUS, buf, sizeof(buf))) { + if (!read_registers(BMP388_REG_STATUS, buf, sizeof(buf))) { return; } const uint8_t status = buf[0]; @@ -212,3 +212,24 @@ void AP_Baro_BMP388::update_pressure(uint32_t data) pressure = press; has_sample = true; } + +/* + read registers, special SPI handling needed +*/ +bool AP_Baro_BMP388::read_registers(uint8_t reg, uint8_t *data, uint8_t len) +{ + // when on I2C we just read normally + if (dev->bus_type() != AP_HAL::Device::BUS_TYPE_SPI) { + return dev->read_registers(reg, data, len); + } + // for SPI we need to discard the first returned byte. See + // datasheet for explanation + uint8_t b[len+2]; + b[0] = reg | 0x80; + memset(&b[1], 0, len+1); + if (!dev->transfer(b, len+2, b, len+2)) { + return false; + } + memcpy(data, &b[2], len); + return true; +} diff --git a/libraries/AP_Baro/AP_Baro_BMP388.h b/libraries/AP_Baro/AP_Baro_BMP388.h index edba1d5b52..ccb24c14d1 100644 --- a/libraries/AP_Baro/AP_Baro_BMP388.h +++ b/libraries/AP_Baro/AP_Baro_BMP388.h @@ -78,4 +78,5 @@ private: } calib; void scale_calibration_data(void); + bool read_registers(uint8_t reg, uint8_t *data, uint8_t len); }; diff --git a/libraries/AP_Baro/AP_Baro_DPS280.cpp b/libraries/AP_Baro/AP_Baro_DPS280.cpp index 0a1792f4b4..af7151a702 100644 --- a/libraries/AP_Baro/AP_Baro_DPS280.cpp +++ b/libraries/AP_Baro/AP_Baro_DPS280.cpp @@ -131,11 +131,16 @@ void AP_Baro_DPS280::set_config_registers(void) bool AP_Baro_DPS280::init() { - if (!dev || !dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + if (!dev) { return false; } + dev->get_semaphore()->take_blocking(); + + // setup to allow reads on SPI + if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) { + dev->set_read_flag(0x80); + } - dev->set_read_flag(0x80); dev->set_speed(AP_HAL::Device::SPEED_HIGH); uint8_t whoami=0; diff --git a/libraries/AP_Baro/AP_Baro_SPL06.cpp b/libraries/AP_Baro/AP_Baro_SPL06.cpp new file mode 100644 index 0000000000..b71f397979 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_SPL06.cpp @@ -0,0 +1,234 @@ +/* + 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_SPL06.h" + +#include + +extern const AP_HAL::HAL &hal; + +#define SPL06_CHIP_ID 0x10 + +#define SPL06_REG_PRESSURE_B2 0x00 // Pressure MSB Register +#define SPL06_REG_PRESSURE_B1 0x01 // Pressure middle byte Register +#define SPL06_REG_PRESSURE_B0 0x02 // Pressure LSB Register +#define SPL06_REG_PRESSURE_START SPL06_REG_PRESSURE_B2 +#define SPL06_PRESSURE_LEN 3 // 24 bits, 3 bytes +#define SPL06_REG_TEMPERATURE_B2 0x03 // Temperature MSB Register +#define SPL06_REG_TEMPERATURE_B1 0x04 // Temperature middle byte Register +#define SPL06_REG_TEMPERATURE_B0 0x05 // Temperature LSB Register +#define SPL06_REG_TEMPERATURE_START SPL06_REG_TEMPERATURE_B2 +#define SPL06_TEMPERATURE_LEN 3 // 24 bits, 3 bytes +#define SPL06_REG_PRESSURE_CFG 0x06 // Pressure config +#define SPL06_REG_TEMPERATURE_CFG 0x07 // Temperature config +#define SPL06_REG_MODE_AND_STATUS 0x08 // Mode and status +#define SPL06_REG_INT_AND_FIFO_CFG 0x09 // Interrupt and FIFO config +#define SPL06_REG_INT_STATUS 0x0A // Interrupt and FIFO config +#define SPL06_REG_FIFO_STATUS 0x0B // Interrupt and FIFO config +#define SPL06_REG_RST 0x0C // Softreset Register +#define SPL06_REG_CHIP_ID 0x0D // Chip ID Register +#define SPL06_REG_CALIB_COEFFS_START 0x10 +#define SPL06_REG_CALIB_COEFFS_END 0x21 + +#define SPL06_CALIB_COEFFS_LEN (SPL06_REG_CALIB_COEFFS_END - SPL06_REG_CALIB_COEFFS_START + 1) + +// TEMPERATURE_CFG_REG +#define SPL06_TEMP_USE_EXT_SENSOR (1<<7) + +// MODE_AND_STATUS_REG +#define SPL06_MEAS_PRESSURE (1<<0) // measure pressure +#define SPL06_MEAS_TEMPERATURE (1<<1) // measure temperature + +#define SPL06_MEAS_CFG_CONTINUOUS (1<<2) +#define SPL06_MEAS_CFG_PRESSURE_RDY (1<<4) +#define SPL06_MEAS_CFG_TEMPERATURE_RDY (1<<5) +#define SPL06_MEAS_CFG_SENSOR_RDY (1<<6) +#define SPL06_MEAS_CFG_COEFFS_RDY (1<<7) + +// INT_AND_FIFO_CFG_REG +#define SPL06_PRESSURE_RESULT_BIT_SHIFT (1<<2) // necessary for pressure oversampling > 8 +#define SPL06_TEMPERATURE_RESULT_BIT_SHIFT (1<<3) // necessary for temperature oversampling > 8 + +// Don't set oversampling higher than 8 or the measurement time will be higher than 20ms (timer period) +#define SPL06_PRESSURE_OVERSAMPLING 8 +#define SPL06_TEMPERATURE_OVERSAMPLING 8 + +#define SPL06_OVERSAMPLING_TO_REG_VALUE(n) (ffs(n)-1) + +AP_Baro_SPL06::AP_Baro_SPL06(AP_Baro &baro, AP_HAL::OwnPtr dev) + : AP_Baro_Backend(baro) + , _dev(std::move(dev)) +{ +} + +AP_Baro_Backend *AP_Baro_SPL06::probe(AP_Baro &baro, + AP_HAL::OwnPtr dev) +{ + if (!dev) { + return nullptr; + } + + if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) { + dev->set_read_flag(0x80); + } + + AP_Baro_SPL06 *sensor = new AP_Baro_SPL06(baro, std::move(dev)); + if (!sensor || !sensor->_init()) { + delete sensor; + return nullptr; + } + return sensor; +} + +bool AP_Baro_SPL06::_init() +{ + if (!_dev) { + return false; + } + WITH_SEMAPHORE(_dev->get_semaphore()); + + _has_sample = false; + + _dev->set_speed(AP_HAL::Device::SPEED_HIGH); + + uint8_t whoami; + if (!_dev->read_registers(SPL06_REG_CHIP_ID, &whoami, 1) || + whoami != SPL06_CHIP_ID) { + // not a SPL06 + return false; + } + + // read the calibration data + uint8_t buf[SPL06_CALIB_COEFFS_LEN]; + _dev->read_registers(SPL06_REG_CALIB_COEFFS_START, buf, sizeof(buf)); + + _c0 = (buf[0] & 0x80 ? 0xF000 : 0) | ((uint16_t)buf[0] << 4) | (((uint16_t)buf[1] & 0xF0) >> 4); + _c1 = ((buf[1] & 0x8 ? 0xF000 : 0) | ((uint16_t)buf[1] & 0x0F) << 8) | (uint16_t)buf[2]; + _c00 = (buf[3] & 0x80 ? 0xFFF00000 : 0) | ((uint32_t)buf[3] << 12) | ((uint32_t)buf[4] << 4) | (((uint32_t)buf[5] & 0xF0) >> 4); + _c10 = (buf[5] & 0x8 ? 0xFFF00000 : 0) | (((uint32_t)buf[5] & 0x0F) << 16) | ((uint32_t)buf[6] << 8) | (uint32_t)buf[7]; + _c01 = ((uint16_t)buf[8] << 8) | ((uint16_t)buf[9]); + _c11 = ((uint16_t)buf[10] << 8) | (uint16_t)buf[11]; + _c20 = ((uint16_t)buf[12] << 8) | (uint16_t)buf[13]; + _c21 = ((uint16_t)buf[14] << 8) | (uint16_t)buf[15]; + _c30 = ((uint16_t)buf[16] << 8) | (uint16_t)buf[17]; + + // setup temperature and pressure measurements + _dev->setup_checked_registers(3, 20); + + _dev->write_register(SPL06_REG_TEMPERATURE_CFG, SPL06_TEMP_USE_EXT_SENSOR | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_TEMPERATURE_OVERSAMPLING), true); + _dev->write_register(SPL06_REG_PRESSURE_CFG, SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_PRESSURE_OVERSAMPLING), true); + + uint8_t int_and_fifo_reg_value = 0; + if (SPL06_TEMPERATURE_OVERSAMPLING > 8) { + int_and_fifo_reg_value |= SPL06_TEMPERATURE_RESULT_BIT_SHIFT; + } + if (SPL06_PRESSURE_OVERSAMPLING > 8) { + int_and_fifo_reg_value |= SPL06_PRESSURE_RESULT_BIT_SHIFT; + } + _dev->write_register(SPL06_REG_INT_AND_FIFO_CFG, int_and_fifo_reg_value, true); + + _instance = _frontend.register_sensor(); + + // request 50Hz update + _timer_counter = -1; + _dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_SPL06::_timer, void)); + + return true; +} + +int32_t AP_Baro_SPL06::raw_value_scale_factor(uint8_t oversampling) +{ + // From the datasheet page 13 + switch(oversampling) + { + case 1: return 524288; + case 2: return 1572864; + case 4: return 3670016; + case 8: return 7864320; + case 16: return 253952; + case 32: return 516096; + case 64: return 1040384; + case 128: return 2088960; + default: return -1; // invalid + } +} + +// acumulate a new sensor reading +void AP_Baro_SPL06::_timer(void) +{ + uint8_t buf[3]; + + if ((_timer_counter == -1) || (_timer_counter == 49)) { + // First call and every second start a temperature measurement (50Hz call) + _dev->write_register(SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_TEMPERATURE, false); + _timer_counter = 0; // Next cycle we are reading the temperature + } else if (_timer_counter == 0) { + // A temperature measurement had been started during the previous call + _dev->read_registers(SPL06_REG_TEMPERATURE_START, buf, sizeof(buf)); + _update_temperature((int32_t)((buf[0] & 0x80 ? 0xFF000000 : 0) | ((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | buf[2])); + _dev->write_register(SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_PRESSURE, false); + _timer_counter += 1; + } else { + // The rest of the time read the latest pressure and start a new measurement + _dev->read_registers(SPL06_REG_PRESSURE_START, buf, sizeof(buf)); + _update_pressure((int32_t)((buf[0] & 0x80 ? 0xFF000000 : 0) | ((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | buf[2])); + _dev->write_register(SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_PRESSURE, false); + _timer_counter += 1; + } + + _dev->check_next_register(); +} + +// transfer data to the frontend +void AP_Baro_SPL06::update(void) +{ + WITH_SEMAPHORE(_sem); + + if (!_has_sample) { + return; + } + + _copy_to_frontend(_instance, _pressure, _temperature); + _has_sample = false; +} + +// calculate temperature +void AP_Baro_SPL06::_update_temperature(int32_t temp_raw) +{ + _temp_raw = (float)temp_raw / raw_value_scale_factor(SPL06_TEMPERATURE_OVERSAMPLING); + const float temp_comp = (float)_c0 / 2 + _temp_raw * _c1; + + WITH_SEMAPHORE(_sem); + + _temperature = temp_comp; +} + +// calculate pressure +void AP_Baro_SPL06::_update_pressure(int32_t press_raw) +{ + const float press_raw_sc = (float)press_raw / raw_value_scale_factor(SPL06_PRESSURE_OVERSAMPLING); + const float pressure_cal = (float)_c00 + press_raw_sc * ((float)_c10 + press_raw_sc * ((float)_c20 + press_raw_sc * _c30)); + const float press_temp_comp = _temp_raw * ((float)_c01 + press_raw_sc * ((float)_c11 + press_raw_sc * _c21)); + + const float press_comp = pressure_cal + press_temp_comp; + + if (!pressure_ok(press_comp)) { + return; + } + + WITH_SEMAPHORE(_sem); + + _pressure = press_comp; + _has_sample = true; +} diff --git a/libraries/AP_Baro/AP_Baro_SPL06.h b/libraries/AP_Baro/AP_Baro_SPL06.h new file mode 100644 index 0000000000..0ab62c2d13 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_SPL06.h @@ -0,0 +1,47 @@ +#pragma once + +#include +#include +#include + +#include "AP_Baro_Backend.h" + +#ifndef HAL_BARO_SPL06_I2C_ADDR + #define HAL_BARO_SPL06_I2C_ADDR (0x76) +#endif +#ifndef HAL_BARO_SPL06_I2C_ADDR2 + #define HAL_BARO_SPL06_I2C_ADDR2 (0x77) +#endif + +class AP_Baro_SPL06 : public AP_Baro_Backend +{ +public: + AP_Baro_SPL06(AP_Baro &baro, AP_HAL::OwnPtr dev); + + /* AP_Baro public interface: */ + void update() override; + + static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr dev); + +private: + + bool _init(void); + void _timer(void); + void _update_temperature(int32_t); + void _update_pressure(int32_t); + + int32_t raw_value_scale_factor(uint8_t); + + AP_HAL::OwnPtr _dev; + + int8_t _timer_counter; + bool _has_sample; + uint8_t _instance; + float _temp_raw; + float _pressure; + float _temperature; + + // Internal calibration registers + int32_t _c00, _c10; + int16_t _c0, _c1, _c01, _c11, _c20, _c21, _c30; +};