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

AP_Baro: add SPA06 to SPx06 driver

This commit is contained in:
Wu 2024-08-28 15:40:23 +08:00 committed by Peter Barker
parent ef5e3c5662
commit c70150e531
2 changed files with 57 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
libraries/AP_Baro/AP_Baro_SPL06.cpp
View File

@ -23,6 +23,7 @@
extern const AP_HAL::HAL &hal; extern const AP_HAL::HAL &hal;
#define SPL06_CHIP_ID 0x10 #define SPL06_CHIP_ID 0x10
#define SPA06_CHIP_ID 0x11
#define SPL06_REG_PRESSURE_B2 0x00 // Pressure MSB Register #define SPL06_REG_PRESSURE_B2 0x00 // Pressure MSB Register
#define SPL06_REG_PRESSURE_B1 0x01 // Pressure middle byte Register #define SPL06_REG_PRESSURE_B1 0x01 // Pressure middle byte Register
@ -44,8 +45,7 @@ extern const AP_HAL::HAL &hal;
#define SPL06_REG_CHIP_ID 0x0D // Chip ID Register #define SPL06_REG_CHIP_ID 0x0D // Chip ID Register
#define SPL06_REG_CALIB_COEFFS_START 0x10 #define SPL06_REG_CALIB_COEFFS_START 0x10
#define SPL06_REG_CALIB_COEFFS_END 0x21 #define SPL06_REG_CALIB_COEFFS_END 0x21
#define SPA06_REG_CALIB_COEFFS_END 0x24
#define SPL06_CALIB_COEFFS_LEN (SPL06_REG_CALIB_COEFFS_END - SPL06_REG_CALIB_COEFFS_START + 1)
// TEMPERATURE_CFG_REG // TEMPERATURE_CFG_REG
#define SPL06_TEMP_USE_EXT_SENSOR (1<<7) #define SPL06_TEMP_USE_EXT_SENSOR (1<<7)
@ -108,21 +108,43 @@ bool AP_Baro_SPL06::_init()
// Sometimes SPL06 has init problems, that's due to failure of reading using SPI for the first time. The SPL06 is a dual // Sometimes SPL06 has init problems, that's due to failure of reading using SPI for the first time. The SPL06 is a dual
// protocol sensor(I2C and SPI), sometimes it takes one SPI operation to convert it to SPI mode after it starts up. // protocol sensor(I2C and SPI), sometimes it takes one SPI operation to convert it to SPI mode after it starts up.
bool is_SPL06 = false;
for (uint8_t i=0; i<5; i++) { for (uint8_t i=0; i<5; i++) {
if (_dev->read_registers(SPL06_REG_CHIP_ID, &whoami, 1) && if (_dev->read_registers(SPL06_REG_CHIP_ID, &whoami, 1)) {
whoami == SPL06_CHIP_ID) { switch(whoami) {
is_SPL06=true; case SPL06_CHIP_ID:
break; type = Type::SPL06;
break;
case SPA06_CHIP_ID:
type = Type::SPA06;
break;
default:
type = Type::UNKNOWN;
break;
}
} }
if (type != Type::UNKNOWN)
break;
} }
if(!is_SPL06) { if (type == Type::UNKNOWN) {
return false; return false;
} }
// read the calibration data // read the calibration data
uint8_t SPL06_CALIB_COEFFS_LEN = 18;
switch(type) {
case Type::SPL06:
SPL06_CALIB_COEFFS_LEN = SPL06_REG_CALIB_COEFFS_END - SPL06_REG_CALIB_COEFFS_START + 1;
break;
case Type::SPA06:
SPL06_CALIB_COEFFS_LEN = SPA06_REG_CALIB_COEFFS_END - SPL06_REG_CALIB_COEFFS_START + 1;
break;
default:
break;
}
uint8_t buf[SPL06_CALIB_COEFFS_LEN]; uint8_t buf[SPL06_CALIB_COEFFS_LEN];
_dev->read_registers(SPL06_REG_CALIB_COEFFS_START, buf, sizeof(buf)); _dev->read_registers(SPL06_REG_CALIB_COEFFS_START, buf, sizeof(buf));
@ -135,6 +157,10 @@ bool AP_Baro_SPL06::_init()
_c20 = ((uint16_t)buf[12] << 8) | (uint16_t)buf[13]; _c20 = ((uint16_t)buf[12] << 8) | (uint16_t)buf[13];
_c21 = ((uint16_t)buf[14] << 8) | (uint16_t)buf[15]; _c21 = ((uint16_t)buf[14] << 8) | (uint16_t)buf[15];
_c30 = ((uint16_t)buf[16] << 8) | (uint16_t)buf[17]; _c30 = ((uint16_t)buf[16] << 8) | (uint16_t)buf[17];
if(type == Type::SPA06) {
_c31 = (buf[18] & 0x80 ? 0xF000 : 0) | ((uint16_t)buf[18] << 4) | (((uint16_t)buf[19] & 0xF0) >> 4);
_c40 = ((buf[19] & 0x8 ? 0xF000 : 0) | ((uint16_t)buf[19] & 0x0F) << 8) | (uint16_t)buf[20];
}
// setup temperature and pressure measurements // setup temperature and pressure measurements
_dev->setup_checked_registers(3, 20); _dev->setup_checked_registers(3, 20);
@ -236,8 +262,21 @@ void AP_Baro_SPL06::_update_temperature(int32_t temp_raw)
void AP_Baro_SPL06::_update_pressure(int32_t press_raw) void AP_Baro_SPL06::_update_pressure(int32_t press_raw)
{ {
const float press_raw_sc = (float)press_raw / raw_value_scale_factor(SPL06_PRESSURE_OVERSAMPLING); const float press_raw_sc = (float)press_raw / raw_value_scale_factor(SPL06_PRESSURE_OVERSAMPLING);
const float pressure_cal = (float)_c00 + press_raw_sc * ((float)_c10 + press_raw_sc * ((float)_c20 + press_raw_sc * _c30)); float pressure_cal = 0;
const float press_temp_comp = _temp_raw * ((float)_c01 + press_raw_sc * ((float)_c11 + press_raw_sc * _c21)); float press_temp_comp = 0;
switch(type) {
case Type::SPL06:
pressure_cal = (float)_c00 + press_raw_sc * ((float)_c10 + press_raw_sc * ((float)_c20 + press_raw_sc * _c30));
press_temp_comp = _temp_raw * ((float)_c01 + press_raw_sc * ((float)_c11 + press_raw_sc * _c21));
break;
case Type::SPA06:
pressure_cal = (float)_c00 + press_raw_sc * ((float)_c10 + press_raw_sc * ((float)_c20 + press_raw_sc * ((float)_c30 + press_raw_sc * _c40)));
press_temp_comp = _temp_raw * ((float)_c01 + press_raw_sc * ((float)_c11 + press_raw_sc * ((float)_c21) + press_raw_sc * _c31));
break;
default:
break;
}
const float press_comp = pressure_cal + press_temp_comp; const float press_comp = pressure_cal + press_temp_comp;

9
libraries/AP_Baro/AP_Baro_SPL06.h
View File

@ -18,6 +18,11 @@
class AP_Baro_SPL06 : public AP_Baro_Backend class AP_Baro_SPL06 : public AP_Baro_Backend
{ {
public: public:
enum class Type {
UNKNOWN,
SPL06,
SPA06,
};
AP_Baro_SPL06(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev); AP_Baro_SPL06(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
/* AP_Baro public interface: */ /* AP_Baro public interface: */
@ -45,7 +50,9 @@ private:
// Internal calibration registers // Internal calibration registers
int32_t _c00, _c10; int32_t _c00, _c10;
int16_t _c0, _c1, _c01, _c11, _c20, _c21, _c30; int16_t _c0, _c1, _c01, _c11, _c20, _c21, _c30, _c31, _c40;
Type type;
}; };
#endif // AP_BARO_SPL06_ENABLED #endif // AP_BARO_SPL06_ENABLED