mirror of https://github.com/ArduPilot/ardupilot
256 lines
7.1 KiB
C++
256 lines
7.1 KiB
C++
/*
|
|
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 <http://www.gnu.org/licenses/>.
|
|
*/
|
|
#include "AP_Baro_BMP388.h"
|
|
|
|
#if AP_BARO_BMP388_ENABLED
|
|
|
|
#include <utility>
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
extern const AP_HAL::HAL &hal;
|
|
|
|
#define BMP388_MODE_SLEEP 0
|
|
#define BMP388_MODE_FORCED 1
|
|
#define BMP388_MODE_NORMAL 3
|
|
#define BMP388_MODE BMP388_MODE_NORMAL
|
|
|
|
#define BMP388_ID 0x50
|
|
#define BMP390_ID 0x60
|
|
|
|
#define BMP388_REG_ID 0x00
|
|
#define BMP388_REV_ID_ADDR 0x01
|
|
#define BMP388_REG_ERR 0x02
|
|
#define BMP388_REG_STATUS 0x03
|
|
#define BMP388_REG_PRESS 0x04 // 24 bit
|
|
#define BMP388_REG_TEMP 0x07 // 24 bit
|
|
#define BMP388_REG_TIME 0x0C // 24 bit
|
|
#define BMP388_REG_EVENT 0x10
|
|
#define BMP388_REG_INT_STS 0x11
|
|
#define BMP388_REG_FIFO_LEN 0x12 // 9 bit
|
|
#define BMP388_REG_FIFO_DATA 0x14
|
|
#define BMP388_REG_FIFO_WTMK 0x15 // 9 bit
|
|
#define BMP388_REG_FIFO_CNF1 0x17
|
|
#define BMP388_REG_FIFO_CNF2 0x18
|
|
#define BMP388_REG_INT_CTRL 0x19
|
|
#define BMP388_REG_PWR_CTRL 0x1B
|
|
#define BMP388_REG_OSR 0x1C
|
|
#define BMP388_REG_ODR 0x1D
|
|
#define BMP388_REG_CONFIG 0x1F
|
|
#define BMP388_REG_CMD 0x7E
|
|
|
|
#define BMP388_REG_CAL_P 0x36
|
|
#define BMP388_REG_CAL_T 0x31
|
|
|
|
AP_Baro_BMP388::AP_Baro_BMP388(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> _dev)
|
|
: AP_Baro_Backend(baro)
|
|
, dev(std::move(_dev))
|
|
{
|
|
}
|
|
|
|
AP_Baro_Backend *AP_Baro_BMP388::probe(AP_Baro &baro,
|
|
AP_HAL::OwnPtr<AP_HAL::Device> _dev)
|
|
{
|
|
if (!_dev) {
|
|
return nullptr;
|
|
}
|
|
|
|
AP_Baro_BMP388 *sensor = NEW_NOTHROW AP_Baro_BMP388(baro, std::move(_dev));
|
|
if (!sensor || !sensor->init()) {
|
|
delete sensor;
|
|
return nullptr;
|
|
}
|
|
return sensor;
|
|
}
|
|
|
|
bool AP_Baro_BMP388::init()
|
|
{
|
|
if (!dev) {
|
|
return false;
|
|
}
|
|
WITH_SEMAPHORE(dev->get_semaphore());
|
|
|
|
dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
|
|
|
// setup to allow reads on SPI
|
|
if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
|
|
dev->set_read_flag(0x80);
|
|
}
|
|
|
|
// normal mode, temp and pressure
|
|
dev->write_register(BMP388_REG_PWR_CTRL, 0x33, true);
|
|
|
|
uint8_t whoami;
|
|
if (!read_registers(BMP388_REG_ID, &whoami, 1)) {
|
|
return false;
|
|
}
|
|
|
|
switch (whoami) {
|
|
case BMP388_ID:
|
|
dev->set_device_type(DEVTYPE_BARO_BMP388);
|
|
break;
|
|
case BMP390_ID:
|
|
dev->set_device_type(DEVTYPE_BARO_BMP390);
|
|
break;
|
|
default:
|
|
return false;
|
|
}
|
|
|
|
// read the calibration data
|
|
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();
|
|
|
|
dev->setup_checked_registers(4);
|
|
|
|
// normal mode, temp and pressure
|
|
dev->write_register(BMP388_REG_PWR_CTRL, 0x33, true);
|
|
|
|
instance = _frontend.register_sensor();
|
|
|
|
set_bus_id(instance, dev->get_bus_id());
|
|
|
|
// request 50Hz update
|
|
dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP388::timer, void));
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
// accumulate a new sensor reading
|
|
void AP_Baro_BMP388::timer(void)
|
|
{
|
|
uint8_t buf[7];
|
|
|
|
if (!read_registers(BMP388_REG_STATUS, buf, sizeof(buf))) {
|
|
return;
|
|
}
|
|
const uint8_t status = buf[0];
|
|
if ((status & 0x20) != 0) {
|
|
// we have pressure data
|
|
update_pressure((buf[3] << 16) | (buf[2] << 8) | buf[1]);
|
|
}
|
|
if ((status & 0x40) != 0) {
|
|
// we have temperature data
|
|
update_temperature((buf[6] << 16) | (buf[5] << 8) | buf[4]);
|
|
}
|
|
|
|
dev->check_next_register();
|
|
}
|
|
|
|
// transfer data to the frontend
|
|
void AP_Baro_BMP388::update(void)
|
|
{
|
|
WITH_SEMAPHORE(_sem);
|
|
|
|
if (pressure_count == 0) {
|
|
return;
|
|
}
|
|
|
|
_copy_to_frontend(instance,
|
|
pressure_sum/pressure_count,
|
|
temperature);
|
|
|
|
pressure_sum = 0;
|
|
pressure_count = 0;
|
|
}
|
|
|
|
/*
|
|
convert calibration data from NVM values to values ready for
|
|
compensation calculations
|
|
*/
|
|
void AP_Baro_BMP388::scale_calibration_data(void)
|
|
{
|
|
// note that this assumes little-endian MCU
|
|
calib.par_t1 = calib_t.nvm_par_t1 * 256.0;
|
|
calib.par_t2 = calib_t.nvm_par_t2 / 1073741824.0f;
|
|
calib.par_t3 = calib_t.nvm_par_t3 / 281474976710656.0f;
|
|
|
|
calib.par_p1 = (calib_p.nvm_par_p1 - 16384) / 1048576.0f;
|
|
calib.par_p2 = (calib_p.nvm_par_p2 - 16384) / 536870912.0f;
|
|
calib.par_p3 = calib_p.nvm_par_p3 / 4294967296.0f;
|
|
calib.par_p4 = calib_p.nvm_par_p4 / 137438953472.0;
|
|
calib.par_p5 = calib_p.nvm_par_p5 * 8.0f;
|
|
calib.par_p6 = calib_p.nvm_par_p6 / 64.0;
|
|
calib.par_p7 = calib_p.nvm_par_p7 / 256.0f;
|
|
calib.par_p8 = calib_p.nvm_par_p8 / 32768.0f;
|
|
calib.par_p9 = calib_p.nvm_par_p9 / 281474976710656.0f;
|
|
calib.par_p10 = calib_p.nvm_par_p10 / 281474976710656.0f;
|
|
calib.par_p11 = calib_p.nvm_par_p11 / 36893488147419103232.0f;
|
|
}
|
|
|
|
/*
|
|
update temperature from raw sample
|
|
*/
|
|
void AP_Baro_BMP388::update_temperature(uint32_t data)
|
|
{
|
|
float partial1 = data - calib.par_t1;
|
|
float partial2 = partial1 * calib.par_t2;
|
|
|
|
WITH_SEMAPHORE(_sem);
|
|
temperature = partial2 + sq(partial1) * calib.par_t3;
|
|
}
|
|
|
|
/*
|
|
update pressure from raw pressure data
|
|
*/
|
|
void AP_Baro_BMP388::update_pressure(uint32_t data)
|
|
{
|
|
float partial1 = calib.par_p6 * temperature;
|
|
float partial2 = calib.par_p7 * powf(temperature, 2);
|
|
float partial3 = calib.par_p8 * powf(temperature, 3);
|
|
float partial_out1 = calib.par_p5 + partial1 + partial2 + partial3;
|
|
|
|
partial1 = calib.par_p2 * temperature;
|
|
partial2 = calib.par_p3 * powf(temperature, 2);
|
|
partial3 = calib.par_p4 * powf(temperature, 3);
|
|
float partial_out2 = data * (calib.par_p1 + partial1 + partial2 + partial3);
|
|
|
|
partial1 = powf(data, 2);
|
|
partial2 = calib.par_p9 + calib.par_p10 * temperature;
|
|
partial3 = partial1 * partial2;
|
|
float partial4 = partial3 + powf(data, 3) * calib.par_p11;
|
|
float press = partial_out1 + partial_out2 + partial4;
|
|
|
|
WITH_SEMAPHORE(_sem);
|
|
|
|
pressure_sum += press;
|
|
pressure_count++;
|
|
}
|
|
|
|
/*
|
|
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;
|
|
}
|
|
|
|
#endif // AP_BARO_BMP388_ENABLED
|