Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: fixes for LPS2XH driver 

use fractional temperature, and check status register to ensure we
have data available
This commit is contained in:
Andrew Tridgell 2019-04-08 10:57:17 +10:00
parent f998a74adf
commit fef42c6219
1 changed files with 26 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_Baro/AP_Baro_LPS2XH.cpp
View File

@ -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()
_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)
// 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)
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)
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