Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: LPS22H: correct formatting

Browse Source
This commit is contained in:
liang 2019-09-24 00:23:07 -07:00 committed by Lucas De Marchi
parent f27b8a6c02
commit d890203f73
1 changed files with 44 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
libraries/AP_Baro/AP_Baro_LPS2XH.cpp
View File

@ -25,24 +25,23 @@ extern const AP_HAL::HAL &hal;
#define LPS22HB_WHOAMI 0xB1 #define LPS22HB_WHOAMI 0xB1
#define LPS25HB_WHOAMI 0xBD #define LPS25HB_WHOAMI 0xBD
#define REG_ID 0x0F #define REG_ID 0x0F
#define LPS22H_ID 0xB1 #define LPS22H_ID 0xB1
#define LPS22H_CTRL_REG1 0x10 #define LPS22H_CTRL_REG1 0x10
#define LPS22H_CTRL_REG2 0x11 #define LPS22H_CTRL_REG2 0x11
#define LPS22H_CTRL_REG3 0x12 #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_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_REG1_ADDR 0x20
#define LPS25H_CTRL_REG2_ADDR 0x21 #define LPS25H_CTRL_REG2_ADDR 0x21
@ -67,11 +66,13 @@ AP_Baro_Backend *AP_Baro_LPS2XH::probe(AP_Baro &baro,
if (!dev) { if (!dev) {
return nullptr; return nullptr;
} }
AP_Baro_LPS2XH *sensor = new AP_Baro_LPS2XH(baro, std::move(dev)); AP_Baro_LPS2XH *sensor = new AP_Baro_LPS2XH(baro, std::move(dev));
if (!sensor || !sensor->_init()) { if (!sensor || !sensor->_init()) {
delete sensor; delete sensor;
return nullptr; return nullptr;
} }
return sensor; return sensor;
} }
@ -82,6 +83,7 @@ AP_Baro_Backend *AP_Baro_LPS2XH::probe_InvensenseIMU(AP_Baro &baro,
if (!dev) { if (!dev) {
return nullptr; return nullptr;
} }
AP_Baro_LPS2XH *sensor = new AP_Baro_LPS2XH(baro, std::move(dev)); AP_Baro_LPS2XH *sensor = new AP_Baro_LPS2XH(baro, std::move(dev));
if (sensor) { if (sensor) {
if (!sensor->_imu_i2c_init(imu_address)) { if (!sensor->_imu_i2c_init(imu_address)) {
@ -89,10 +91,12 @@ AP_Baro_Backend *AP_Baro_LPS2XH::probe_InvensenseIMU(AP_Baro &baro,
return nullptr; return nullptr;
} }
} }
if (!sensor || !sensor->_init()) { if (!sensor || !sensor->_init()) {
delete sensor; delete sensor;
return nullptr; return nullptr;
} }
return sensor; 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)) { if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false; return false;
} }
// as the baro device is already locked we need to re-use it, // as the baro device is already locked we need to re-use it,
// changing its address to match the IMU address // changing its address to match the IMU address
uint8_t old_address = _dev->get_bus_address(); uint8_t old_address = _dev->get_bus_address();
_dev->set_address(imu_address); _dev->set_address(imu_address);
_dev->set_retries(4); _dev->set_retries(4);
uint8_t whoami=0; 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_FIFO_EN, 0x00);
_dev->write_register(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_XGYRO); _dev->write_register(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_XGYRO);
// wait for sensor to settle // wait for sensor to settle
hal.scheduler->delay(10); hal.scheduler->delay(10);
@ -136,20 +141,21 @@ bool AP_Baro_LPS2XH::_init()
if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false; return false;
} }
_has_sample = false; _has_sample = false;
_dev->set_speed(AP_HAL::Device::SPEED_HIGH); _dev->set_speed(AP_HAL::Device::SPEED_HIGH);
// top bit is for read on SPI // top bit is for read on SPI
_dev->set_read_flag(0x80); _dev->set_read_flag(0x80);
if(!_check_whoami()){ if (!_check_whoami()) {
_dev->get_semaphore()->give(); _dev->get_semaphore()->give();
return false; return false;
} }
//init control registers. //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_REG1_ADDR,0x00); // turn off for config
_dev->write_register(LPS25H_CTRL_REG2_ADDR,0x00); //FIFO Disabled _dev->write_register(LPS25H_CTRL_REG2_ADDR,0x00); //FIFO Disabled
_dev->write_register(LPS25H_FIFO_CTRL, 0x01); _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) // request 25Hz update (maximum refresh Rate according to datasheet)
CallTime = 40 * AP_USEC_PER_MSEC; 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, 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); _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) { 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); _dev->write_register(LPS22H_CTRL_REG2, 0x10);
} }
// request 75Hz update // request 75Hz update
CallTime = 1000000/75; CallTime = 1000000/75;
} }
@ -184,23 +191,23 @@ bool AP_Baro_LPS2XH::_init()
bool AP_Baro_LPS2XH::_check_whoami(void) bool AP_Baro_LPS2XH::_check_whoami(void)
{ {
uint8_t whoami; uint8_t whoami;
if (! _dev->read_registers(REG_ID, &whoami, 1)) { if (!_dev->read_registers(REG_ID, &whoami, 1)) {
return false; return false;
} }
hal.console->printf("LPS2XH whoami 0x%02x\n", whoami); hal.console->printf("LPS2XH whoami 0x%02x\n", whoami);
switch(whoami){ switch(whoami){
case LPS22HB_WHOAMI: case LPS22HB_WHOAMI:
_lps2xh_type = BARO_LPS22H; _lps2xh_type = BARO_LPS22H;
return true; return true;
case LPS25HB_WHOAMI: case LPS25HB_WHOAMI:
_lps2xh_type = BARO_LPS25H; _lps2xh_type = BARO_LPS25H;
return true; return true;
} }
return false; return false;
} }
// acumulate a new sensor reading // acumulate a new sensor reading
void AP_Baro_LPS2XH::_timer(void) void AP_Baro_LPS2XH::_timer(void)
{ {
@ -209,12 +216,15 @@ void AP_Baro_LPS2XH::_timer(void)
if (!_dev->read_registers(STATUS_ADDR, &status, 1)) { if (!_dev->read_registers(STATUS_ADDR, &status, 1)) {
return; return;
} }
if (status & 0x02) { if (status & 0x02) {
_update_temperature(); _update_temperature();
} }
if (status & 0x01) { if (status & 0x01) {
_update_pressure(); _update_pressure();
} }
_has_sample = true; _has_sample = true;
} }
@ -227,6 +237,7 @@ void AP_Baro_LPS2XH::update(void)
WITH_SEMAPHORE(_sem); WITH_SEMAPHORE(_sem);
_copy_to_frontend(_instance, _pressure, _temperature); _copy_to_frontend(_instance, _pressure, _temperature);
_has_sample = false; _has_sample = false;
} }
@ -238,12 +249,13 @@ void AP_Baro_LPS2XH::_update_temperature(void)
return; return;
} }
int16_t Temp_Reg_s16 = (uint16_t)(pu8[1]<<8) | pu8[0]; int16_t Temp_Reg_s16 = (uint16_t)(pu8[1]<<8) | pu8[0];
WITH_SEMAPHORE(_sem); WITH_SEMAPHORE(_sem);
if (_lps2xh_type == BARO_LPS25H) { if (_lps2xh_type == BARO_LPS25H) {
_temperature = (Temp_Reg_s16 * (1.0/480)) + 42.5; _temperature = (Temp_Reg_s16 * (1.0/480)) + 42.5;
} }
if (_lps2xh_type == BARO_LPS22H) { if (_lps2xh_type == BARO_LPS22H) {
_temperature = Temp_Reg_s16 * 0.01; _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)) { if (!_dev->read_registers(PRESS_OUT_XL_ADDR, pressure, 3)) {
return; return;
} }
int32_t Pressure_Reg_s32 = ((uint32_t)pressure[2]<<16)|((uint32_t)pressure[1]<<8)|(uint32_t)pressure[0]; 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 int32_t Pressure_mb = Pressure_Reg_s32 * (100.0f / 4096); // scale for pa