Andrew Tridgell
6 years ago
4 changed files with 307 additions and 2 deletions
@ -0,0 +1,214 @@
@@ -0,0 +1,214 @@
|
||||
/*
|
||||
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" |
||||
|
||||
#include <utility> |
||||
|
||||
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 BMP388_REG_ID 0x00 |
||||
#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 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()); |
||||
|
||||
has_sample = false; |
||||
|
||||
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 (!dev->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)); |
||||
|
||||
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(); |
||||
|
||||
// request 50Hz update
|
||||
dev->register_periodic_callback(20 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_BMP388::timer, void)); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
|
||||
|
||||
// acumulate a new sensor reading
|
||||
void AP_Baro_BMP388::timer(void) |
||||
{ |
||||
uint8_t buf[7]; |
||||
|
||||
if (!dev->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 (!has_sample) { |
||||
return; |
||||
} |
||||
|
||||
_copy_to_frontend(instance, pressure, temperature); |
||||
has_sample = false; |
||||
} |
||||
|
||||
/*
|
||||
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 = press; |
||||
has_sample = true; |
||||
} |
||||
@ -0,0 +1,81 @@
@@ -0,0 +1,81 @@
|
||||
#pragma once |
||||
|
||||
#include <AP_HAL/AP_HAL.h> |
||||
#include <AP_HAL/Device.h> |
||||
#include <AP_HAL/utility/OwnPtr.h> |
||||
|
||||
#include "AP_Baro_Backend.h" |
||||
|
||||
#ifndef HAL_BARO_BMP388_I2C_ADDR |
||||
#define HAL_BARO_BMP388_I2C_ADDR (0x76) |
||||
#endif |
||||
#ifndef HAL_BARO_BMP388_I2C_ADDR2 |
||||
#define HAL_BARO_BMP388_I2C_ADDR2 (0x77) |
||||
#endif |
||||
|
||||
class AP_Baro_BMP388 : public AP_Baro_Backend |
||||
{ |
||||
public: |
||||
AP_Baro_BMP388(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> _dev); |
||||
|
||||
/* AP_Baro public interface: */ |
||||
void update() override; |
||||
|
||||
static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> _dev); |
||||
|
||||
private: |
||||
|
||||
bool init(void); |
||||
void timer(void); |
||||
void update_temperature(uint32_t); |
||||
void update_pressure(uint32_t); |
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev; |
||||
|
||||
bool has_sample; |
||||
uint8_t instance; |
||||
float pressure; |
||||
float temperature; |
||||
|
||||
// Internal calibration registers
|
||||
struct PACKED { |
||||
int16_t nvm_par_p1; // at 0x36
|
||||
int16_t nvm_par_p2; |
||||
int8_t nvm_par_p3; |
||||
int8_t nvm_par_p4; |
||||
int16_t nvm_par_p5; |
||||
int16_t nvm_par_p6; |
||||
int8_t nvm_par_p7; |
||||
int8_t nvm_par_p8; |
||||
int16_t nvm_par_p9; |
||||
int8_t nvm_par_p10; |
||||
int8_t nvm_par_p11; |
||||
} calib_p; |
||||
|
||||
struct PACKED { |
||||
uint16_t nvm_par_t1; // at 0x31
|
||||
uint16_t nvm_par_t2; |
||||
int8_t nvm_par_t3; |
||||
} calib_t; |
||||
|
||||
// scaled calibration data
|
||||
struct { |
||||
float par_t1; |
||||
float par_t2; |
||||
float par_t3; |
||||
float par_p1; |
||||
float par_p2; |
||||
float par_p3; |
||||
float par_p4; |
||||
float par_p5; |
||||
float par_p6; |
||||
float par_p7; |
||||
float par_p8; |
||||
float par_p9; |
||||
float par_p10; |
||||
float par_p11; |
||||
float t_lin; |
||||
} calib; |
||||
|
||||
void scale_calibration_data(void); |
||||
}; |
||||
Loading…
Reference in new issue