mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 14:38:30 -04:00
AP_Baro: LPS22H: correct formatting
This commit is contained in:
parent
f27b8a6c02
commit
d890203f73
@ -37,13 +37,12 @@ extern const AP_HAL::HAL &hal;
|
||||
#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_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
|
||||
#define LPS25H_CTRL_REG3_ADDR 0x22
|
||||
@ -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,6 +109,7 @@ 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();
|
||||
@ -136,6 +141,7 @@ 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);
|
||||
@ -143,13 +149,13 @@ bool AP_Baro_LPS2XH::_init()
|
||||
// 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) {
|
||||
@ -184,7 +191,7 @@ 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);
|
||||
@ -197,10 +204,10 @@ bool AP_Baro_LPS2XH::_check_whoami(void)
|
||||
_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;
|
||||
}
|
||||
|
||||
@ -244,6 +255,7 @@ void AP_Baro_LPS2XH::_update_temperature(void)
|
||||
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
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user