|
|
|
@ -47,10 +47,12 @@ extern const AP_HAL::HAL &hal;
@@ -47,10 +47,12 @@ extern const AP_HAL::HAL &hal;
|
|
|
|
|
#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 |
|
|
|
|
#define LPS25H_CTRL_REG4_ADDR 0x23 |
|
|
|
|
#define LPS25H_FIFO_CTRL 0x2E |
|
|
|
|
#define TEMP_OUT_ADDR 0x2B |
|
|
|
|
#define PRESS_OUT_XL_ADDR 0x28 |
|
|
|
|
#define STATUS_ADDR 0x27 |
|
|
|
|
|
|
|
|
|
//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<AP_HAL::Device> dev) |
|
|
|
@ -161,7 +163,7 @@ bool AP_Baro_LPS2XH::_init()
@@ -161,7 +163,7 @@ bool AP_Baro_LPS2XH::_init()
|
|
|
|
|
_dev->write_register(LPS22H_CTRL_REG2,0x18); |
|
|
|
|
|
|
|
|
|
// request 75Hz update
|
|
|
|
|
CallTime = 1000000/75; |
|
|
|
|
CallTime = 1000000/75; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
_instance = _frontend.register_sensor(); |
|
|
|
@ -197,8 +199,17 @@ bool AP_Baro_LPS2XH::_check_whoami(void)
@@ -197,8 +199,17 @@ bool AP_Baro_LPS2XH::_check_whoami(void)
|
|
|
|
|
// acumulate a new sensor reading
|
|
|
|
|
void AP_Baro_LPS2XH::_timer(void) |
|
|
|
|
{ |
|
|
|
|
_update_temperature(); |
|
|
|
|
_update_pressure(); |
|
|
|
|
uint8_t status; |
|
|
|
|
// use status to check if data is available
|
|
|
|
|
if (!_dev->read_registers(STATUS_ADDR, &status, 1)) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
if (status & 0x02) { |
|
|
|
|
_update_temperature(); |
|
|
|
|
} |
|
|
|
|
if (status & 0x01) { |
|
|
|
|
_update_pressure(); |
|
|
|
|
} |
|
|
|
|
_has_sample = true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -218,16 +229,18 @@ void AP_Baro_LPS2XH::update(void)
@@ -218,16 +229,18 @@ void AP_Baro_LPS2XH::update(void)
|
|
|
|
|
void AP_Baro_LPS2XH::_update_temperature(void) |
|
|
|
|
{ |
|
|
|
|
uint8_t pu8[2]; |
|
|
|
|
_dev->read_registers(TEMP_OUT_ADDR, pu8, 2); |
|
|
|
|
if (!_dev->read_registers(TEMP_OUT_ADDR, pu8, 2)) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
int16_t Temp_Reg_s16 = (uint16_t)(pu8[1]<<8) | pu8[0]; |
|
|
|
|
|
|
|
|
|
WITH_SEMAPHORE(_sem); |
|
|
|
|
|
|
|
|
|
if (_lps2xh_type == BARO_LPS25H) { |
|
|
|
|
_temperature=((float)(Temp_Reg_s16/480)+42.5f); |
|
|
|
|
_temperature = (Temp_Reg_s16 * (1.0/480)) + 42.5; |
|
|
|
|
} |
|
|
|
|
if (_lps2xh_type == BARO_LPS22H) { |
|
|
|
|
_temperature=(float)(Temp_Reg_s16/100); |
|
|
|
|
_temperature = Temp_Reg_s16 * 0.01; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -235,7 +248,9 @@ void AP_Baro_LPS2XH::_update_temperature(void)
@@ -235,7 +248,9 @@ void AP_Baro_LPS2XH::_update_temperature(void)
|
|
|
|
|
void AP_Baro_LPS2XH::_update_pressure(void) |
|
|
|
|
{ |
|
|
|
|
uint8_t pressure[3]; |
|
|
|
|
_dev->read_registers(PRESS_OUT_XL_ADDR, pressure, 3); |
|
|
|
|
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
|
|
|
|
|
|
|
|
|
|