diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 25c392a62f..6155a09ade 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -34,8 +34,8 @@ #include "AP_Baro_HIL.h" #include "AP_Baro_KellerLD.h" #include "AP_Baro_MS5611.h" -#include "AP_Baro_LPS25H.h" #include "AP_Baro_ICM20789.h" +#include "AP_Baro_LPS2XH.h" #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT #include "AP_Baro_qflight.h" #endif @@ -519,7 +519,7 @@ void AP_Baro::init(void) drivers[0] = new AP_Baro_QURT(*this); _num_drivers = 1; #elif HAL_BARO_DEFAULT == HAL_BARO_LPS25H - ADD_BACKEND(AP_Baro_LPS25H::probe(*this, + ADD_BACKEND(AP_Baro_LPS2XH::probe(*this, std::move(hal.i2c_mgr->get_device(HAL_BARO_LPS25H_I2C_BUS, HAL_BARO_LPS25H_I2C_ADDR)))); #elif HAL_BARO_DEFAULT == HAL_BARO_20789_I2C_I2C ADD_BACKEND(AP_Baro_ICM20789::probe(*this, @@ -529,6 +529,9 @@ void AP_Baro::init(void) ADD_BACKEND(AP_Baro_ICM20789::probe(*this, std::move(hal.i2c_mgr->get_device(HAL_BARO_20789_I2C_BUS, HAL_BARO_20789_I2C_ADDR_PRESS)), std::move(hal.spi->get_device("icm20789")))); +#elif HAL_BARO_DEFAULT == HAL_BARO_LPS22H_SPI + ADD_BACKEND(AP_Baro_LPS2XH::probe(*this, + std::move(hal.spi->get_device(HAL_BARO_LPS22H_NAME)))); #endif // can optionally have baro on I2C too diff --git a/libraries/AP_Baro/AP_Baro_LPS25H.cpp b/libraries/AP_Baro/AP_Baro_LPS25H.cpp deleted file mode 100644 index 46bcf9c1db..0000000000 --- a/libraries/AP_Baro/AP_Baro_LPS25H.cpp +++ /dev/null @@ -1,135 +0,0 @@ -/* - 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_LPS25H.h" - -#include -#include -extern const AP_HAL::HAL &hal; - - -#define LPS25H_ID 0xBD -#define LPS25H_REG_ID 0x0F -#define LPS25H_CTRL_REG1_ADDR 0x20 -#define LPS25H_CTRL_REG2_ADDR 0x21 -#define LPS25H_CTRL_REG3_ADDR 0x22 -#define LPS25H_CTRL_REG4_ADDR 0x23 -#define LPS25H_FIFO_CTRL 0x2E -#define LPS25H_TEMP_OUT_ADDR 0xAB //Regsiter address is 0x2B in 2's compliment. -#define PRESS_OUT_XL_ADDR 0xA8 //Regsiter address is 0x28 in 2's compliment. -//putting 1 in the MSB of those two registers turns on Auto increment for faster reading. - -AP_Baro_LPS25H::AP_Baro_LPS25H(AP_Baro &baro, AP_HAL::OwnPtr dev) - : AP_Baro_Backend(baro) - , _dev(std::move(dev)) -{ -} - -AP_Baro_Backend *AP_Baro_LPS25H::probe(AP_Baro &baro, - AP_HAL::OwnPtr dev) -{ - if (!dev) { - return nullptr; - } - AP_Baro_LPS25H *sensor = new AP_Baro_LPS25H(baro, std::move(dev)); - if (!sensor || !sensor->_init()) { - delete sensor; - return nullptr; - } - return sensor; -} - -bool AP_Baro_LPS25H::_init() -{ - if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { - return false; - } - _has_sample = false; - - _dev->set_speed(AP_HAL::Device::SPEED_HIGH); - - uint8_t whoami; - if (!_dev->read_registers(LPS25H_REG_ID, &whoami, 1) || - whoami != LPS25H_ID) { - // not a LPS25H - _dev->get_semaphore()->give(); - return false; - } - - //init control registers. - _dev->write_register(LPS25H_CTRL_REG1_ADDR,0x00); // turn off for config - _dev->write_register(LPS25H_CTRL_REG2_ADDR,0x00); //FIFO Disabled - _dev->write_register(LPS25H_FIFO_CTRL, 0x01); - _dev->write_register(LPS25H_CTRL_REG1_ADDR,0xc0); - - _instance = _frontend.register_sensor(); - - _dev->get_semaphore()->give(); - - // request 25Hz update (maximum refresh Rate according to datasheet) - _dev->register_periodic_callback(40 * AP_USEC_PER_MSEC, FUNCTOR_BIND_MEMBER(&AP_Baro_LPS25H::_timer, void)); - - return true; -} - - - -// acumulate a new sensor reading -void AP_Baro_LPS25H::_timer(void) -{ - _update_temperature(); - _update_pressure(); - _has_sample = true; -} - -// transfer data to the frontend -void AP_Baro_LPS25H::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_LPS25H::_update_temperature(void) -{ - uint8_t pu8[2]; - _dev->read_registers(LPS25H_TEMP_OUT_ADDR, pu8, 2); - int16_t Temp_Reg_s16 = (uint16_t)(pu8[1]<<8) | pu8[0]; - if (_sem->take_nonblocking()) { - _temperature=((float)(Temp_Reg_s16/480)+42.5); - _sem->give(); - } - -} - -// calculate pressure -void AP_Baro_LPS25H::_update_pressure(void) -{ - uint8_t pressure[3]; - _dev->read_registers(PRESS_OUT_XL_ADDR, pressure, 3); - int32_t Pressure_Reg_s32 = ((uint32_t)pressure[2]<<16)|((uint32_t)pressure[1]<<8)|(uint32_t)pressure[0]; - int32_t Pressure_mb = Pressure_Reg_s32 / 4096; // scale - if (_sem->take_nonblocking()) { - _pressure=Pressure_mb; - _sem->give(); - } -} diff --git a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp new file mode 100644 index 0000000000..4833265d54 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp @@ -0,0 +1,185 @@ +/* + 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 +#include + +#include "AP_Baro_LPS2XH.h" +extern const AP_HAL::HAL &hal; + +#define REG_ID 0x0F + +#define LPS22H_ID 0xB1 +#define LPS22H_CTRL_REG1 0x10 +#define LPS22H_CTRL_REG2 0x11 +#define LPS22H_CTRL_REG3 0x12 + +#define LPS22H_CTRL_REG1_SIM (1 << 0) +#define LPS22H_CTRL_REG1_BDU (1 << 1) +#define LPS22H_CTRL_REG1_LPFP_CFG (1 << 2) +#define LPS22H_CTRL_REG1_EN_LPFP (1 << 3) +#define LPS22H_CTRL_REG1_PD (0 << 4) +#define LPS22H_CTRL_REG1_ODR_1HZ (1 << 4) +#define LPS22H_CTRL_REG1_ODR_10HZ (2 << 4) +#define LPS22H_CTRL_REG1_ODR_25HZ (3 << 4) +#define LPS22H_CTRL_REG1_ODR_50HZ (4 << 4) +#define LPS22H_CTRL_REG1_ODR_75HZ (5 << 4) + + +#define LPS25H_CTRL_REG1_ADDR 0x20 +#define LPS25H_CTRL_REG2_ADDR 0x21 +#define LPS25H_CTRL_REG3_ADDR 0x22 +#define LPS25H_CTRL_REG4_ADDR 0x23 +#define LPS25H_FIFO_CTRL 0x2E +#define TEMP_OUT_ADDR 0x2B +#define PRESS_OUT_XL_ADDR 0x28 +//putting 1 in the MSB of those two registers turns on Auto increment for faster reading. + +AP_Baro_LPS2XH::AP_Baro_LPS2XH(AP_Baro &baro, AP_HAL::OwnPtr dev) + : AP_Baro_Backend(baro) + , _dev(std::move(dev)) +{ +} + +AP_Baro_Backend *AP_Baro_LPS2XH::probe(AP_Baro &baro, + AP_HAL::OwnPtr dev) +{ + if (!dev) { + return nullptr; + } + AP_Baro_LPS2XH *sensor = new AP_Baro_LPS2XH(baro, std::move(dev)); + if (!sensor || !sensor->_init()) { + delete sensor; + return nullptr; + } + return sensor; +} + +bool AP_Baro_LPS2XH::_init() +{ + if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { + return false; + } + _has_sample = false; + + _dev->set_speed(AP_HAL::Device::SPEED_HIGH); + + // top bit is for read on SPI + _dev->set_read_flag(0x80); + + if(!_check_whoami()){ + _dev->get_semaphore()->give(); + return false; + } + + //init control registers. + if(_lps2xh_type == BARO_LPS25H){ + _dev->write_register(LPS25H_CTRL_REG1_ADDR,0x00); // turn off for config + _dev->write_register(LPS25H_CTRL_REG2_ADDR,0x00); //FIFO Disabled + _dev->write_register(LPS25H_FIFO_CTRL, 0x01); + _dev->write_register(LPS25H_CTRL_REG1_ADDR,0xc0); + + // request 25Hz update (maximum refresh Rate according to datasheet) + CallTime = 40 * AP_USEC_PER_MSEC; + } + if(_lps2xh_type == BARO_LPS22H){ + _dev->write_register(LPS22H_CTRL_REG1,LPS22H_CTRL_REG1_ODR_75HZ|LPS22H_CTRL_REG1_BDU|LPS22H_CTRL_REG1_EN_LPFP|LPS22H_CTRL_REG1_LPFP_CFG); + _dev->write_register(LPS22H_CTRL_REG2,0x18); + + // request 75Hz update + CallTime = 1000000/75; + } + + _instance = _frontend.register_sensor(); + + _dev->get_semaphore()->give(); + + _dev->register_periodic_callback(CallTime, FUNCTOR_BIND_MEMBER(&AP_Baro_LPS2XH::_timer, void)); + + return true; +} + +//check ID +bool AP_Baro_LPS2XH::_check_whoami(void) +{ + uint8_t whoami; + if (! _dev->read_registers(REG_ID, &whoami, 1)) { + return false; + } + hal.console->printf("LPS2XH whoami 0x%02x\n", whoami); + + switch(whoami){ + case LPS22HB_WHOAMI: + _lps2xh_type = BARO_LPS22H; + return true; + case LPS25HB_WHOAMI: + _lps2xh_type = BARO_LPS25H; + return true; + } + return false; +} + + +// acumulate a new sensor reading +void AP_Baro_LPS2XH::_timer(void) +{ + _update_temperature(); + _update_pressure(); + _has_sample = true; +} + +// transfer data to the frontend +void AP_Baro_LPS2XH::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_LPS2XH::_update_temperature(void) +{ + uint8_t pu8[2]; + _dev->read_registers(TEMP_OUT_ADDR, pu8, 2); + int16_t Temp_Reg_s16 = (uint16_t)(pu8[1]<<8) | pu8[0]; + if (_sem->take_nonblocking()) { + if(_lps2xh_type == BARO_LPS25H){ + _temperature=((float)(Temp_Reg_s16/480)+42.5); + } + if(_lps2xh_type == BARO_LPS22H){ + _temperature=(float)(Temp_Reg_s16/100); + } + _sem->give(); + } +} + +// calculate pressure +void AP_Baro_LPS2XH::_update_pressure(void) +{ + uint8_t pressure[3]; + _dev->read_registers(PRESS_OUT_XL_ADDR, pressure, 3); + int32_t Pressure_Reg_s32 = ((uint32_t)pressure[2]<<16)|((uint32_t)pressure[1]<<8)|(uint32_t)pressure[0]; + int32_t Pressure_mb = Pressure_Reg_s32 / 4096 *100; // scale for pa + if (_sem->take_nonblocking()) { + _pressure=Pressure_mb; + _sem->give(); + } +} diff --git a/libraries/AP_Baro/AP_Baro_LPS25H.h b/libraries/AP_Baro/AP_Baro_LPS2XH.h similarity index 60% rename from libraries/AP_Baro/AP_Baro_LPS25H.h rename to libraries/AP_Baro/AP_Baro_LPS2XH.h index 05daf0c97c..e4936afe1a 100644 --- a/libraries/AP_Baro/AP_Baro_LPS25H.h +++ b/libraries/AP_Baro/AP_Baro_LPS2XH.h @@ -10,27 +10,44 @@ #define HAL_BARO_LPS25H_I2C_ADDR 0x5D -class AP_Baro_LPS25H : public AP_Baro_Backend +class AP_Baro_LPS2XH : public AP_Baro_Backend { public: - AP_Baro_LPS25H(AP_Baro &baro, AP_HAL::OwnPtr dev); + enum LPS2XH_TYPE { + BARO_LPS22H = 0, + BARO_LPS25H = 1, + }; + + AP_Baro_LPS2XH(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_LPS2XH(void) {}; bool _init(void); void _timer(void); void _update_temperature(void); void _update_pressure(void); + bool _check_whoami(void); + AP_HAL::OwnPtr _dev; bool _has_sample; uint8_t _instance; float _pressure; float _temperature; + + uint32_t CallTime = 0; + + enum LPS2XH_TYPE _lps2xh_type; + + // WHOAMI values +#define LPS22HB_WHOAMI 0xB1 +#define LPS25HB_WHOAMI 0xBD };