diff --git a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp index d4e5fbabbb..627d4ae117 100644 --- a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp +++ b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp @@ -25,24 +25,23 @@ extern const AP_HAL::HAL &hal; #define LPS22HB_WHOAMI 0xB1 #define LPS25HB_WHOAMI 0xBD -#define REG_ID 0x0F +#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 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_1H (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 @@ -67,11 +66,13 @@ AP_Baro_Backend *AP_Baro_LPS2XH::probe(AP_Baro &baro, 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; } @@ -82,6 +83,7 @@ AP_Baro_Backend *AP_Baro_LPS2XH::probe_InvensenseIMU(AP_Baro &baro, if (!dev) { return nullptr; } + AP_Baro_LPS2XH *sensor = new AP_Baro_LPS2XH(baro, std::move(dev)); if (sensor) { if (!sensor->_imu_i2c_init(imu_address)) { @@ -89,10 +91,12 @@ AP_Baro_Backend *AP_Baro_LPS2XH::probe_InvensenseIMU(AP_Baro &baro, return nullptr; } } + if (!sensor || !sensor->_init()) { delete sensor; return nullptr; } + return sensor; } @@ -105,11 +109,12 @@ bool AP_Baro_LPS2XH::_imu_i2c_init(uint8_t imu_address) if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { return false; } + // as the baro device is already locked we need to re-use it, // changing its address to match the IMU address uint8_t old_address = _dev->get_bus_address(); _dev->set_address(imu_address); - + _dev->set_retries(4); uint8_t whoami=0; @@ -118,7 +123,7 @@ bool AP_Baro_LPS2XH::_imu_i2c_init(uint8_t imu_address) _dev->write_register(MPUREG_FIFO_EN, 0x00); _dev->write_register(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_XGYRO); - + // wait for sensor to settle hal.scheduler->delay(10); @@ -136,20 +141,21 @@ 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()){ + + if (!_check_whoami()) { _dev->get_semaphore()->give(); return false; } //init control registers. - if(_lps2xh_type == BARO_LPS25H){ + 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); @@ -158,7 +164,8 @@ bool AP_Baro_LPS2XH::_init() // request 25Hz update (maximum refresh Rate according to datasheet) CallTime = 40 * AP_USEC_PER_MSEC; } - if(_lps2xh_type == BARO_LPS22H){ + + if (_lps2xh_type == BARO_LPS22H) { _dev->write_register(LPS22H_CTRL_REG1, 0x00); // turn off for config _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); if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) { @@ -167,7 +174,7 @@ bool AP_Baro_LPS2XH::_init() _dev->write_register(LPS22H_CTRL_REG2, 0x10); } - // request 75Hz update + // request 75Hz update CallTime = 1000000/75; } @@ -184,23 +191,23 @@ bool AP_Baro_LPS2XH::_init() bool AP_Baro_LPS2XH::_check_whoami(void) { uint8_t whoami; - if (! _dev->read_registers(REG_ID, &whoami, 1)) { + 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 LPS22HB_WHOAMI: + _lps2xh_type = BARO_LPS22H; + return true; case LPS25HB_WHOAMI: - _lps2xh_type = BARO_LPS25H; - return true; + _lps2xh_type = BARO_LPS25H; + return true; } + return false; } - // acumulate a new sensor reading void AP_Baro_LPS2XH::_timer(void) { @@ -209,12 +216,15 @@ void AP_Baro_LPS2XH::_timer(void) if (!_dev->read_registers(STATUS_ADDR, &status, 1)) { return; } + if (status & 0x02) { _update_temperature(); } + if (status & 0x01) { _update_pressure(); } + _has_sample = true; } @@ -227,6 +237,7 @@ void AP_Baro_LPS2XH::update(void) WITH_SEMAPHORE(_sem); _copy_to_frontend(_instance, _pressure, _temperature); + _has_sample = false; } @@ -238,12 +249,13 @@ void AP_Baro_LPS2XH::_update_temperature(void) return; } int16_t Temp_Reg_s16 = (uint16_t)(pu8[1]<<8) | pu8[0]; - + WITH_SEMAPHORE(_sem); if (_lps2xh_type == BARO_LPS25H) { _temperature = (Temp_Reg_s16 * (1.0/480)) + 42.5; } + if (_lps2xh_type == BARO_LPS22H) { _temperature = Temp_Reg_s16 * 0.01; } @@ -256,6 +268,7 @@ void AP_Baro_LPS2XH::_update_pressure(void) if (!_dev->read_registers(PRESS_OUT_XL_ADDR, pressure, 3)) { return; } + 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 * (100.0f / 4096); // scale for pa