Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Rework SPL06 to do background updates compliant with the spec #28983

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
+62 −15
Open

Rework SPL06 to do background updates compliant with the spec #28983

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
77 changes: 62 additions & 15 deletions libraries/AP_Baro/AP_Baro_SPL06.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ extern const AP_HAL::HAL &hal;

// enable Background Mode for continuous measurement
#ifndef AP_BARO_SPL06_BACKGROUND_ENABLE
#define AP_BARO_SPL06_BACKGROUND_ENABLE 0
#define AP_BARO_SPL06_BACKGROUND_ENABLE 1
#endif

AP_Baro_SPL06::AP_Baro_SPL06(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev)
Expand Down Expand Up @@ -107,6 +107,16 @@ AP_Baro_Backend *AP_Baro_SPL06::probe(AP_Baro &baro,
return sensor;
}

static int32_t get_twos_complement(uint32_t raw, uint8_t length)
{
if (raw & ((int)1 << (length - 1))) {
return ((int32_t)raw) - ((int32_t)1 << length);
}
else {
return raw;
}
}

bool AP_Baro_SPL06::_init()
{
if (!_dev) {
Expand Down Expand Up @@ -157,29 +167,66 @@ bool AP_Baro_SPL06::_init()
break;
}

bool ready = false;
for (uint8_t i=0; i<5; i++) {
uint8_t status = 0;
if (_dev->read_registers(SPL06_REG_MODE_AND_STATUS, &status, 1)) {
if ((status & 1<<7U) && (status & 1<<6U)) {
ready = true;
break;
}
}
hal.scheduler->delay_microseconds(100);
}

if (!ready) {
return false;
}

uint8_t buf[SPL06_CALIB_COEFFS_LEN];
_dev->read_registers(SPL06_REG_CALIB_COEFFS_START, buf, sizeof(buf));

_c0 = (buf[0] & 0x80 ? 0xF000 : 0) | ((uint16_t)buf[0] << 4) | (((uint16_t)buf[1] & 0xF0) >> 4);
_c1 = ((buf[1] & 0x8 ? 0xF000 : 0) | ((uint16_t)buf[1] & 0x0F) << 8) | (uint16_t)buf[2];
_c00 = (buf[3] & 0x80 ? 0xFFF00000 : 0) | ((uint32_t)buf[3] << 12) | ((uint32_t)buf[4] << 4) | (((uint32_t)buf[5] & 0xF0) >> 4);
_c10 = (buf[5] & 0x8 ? 0xFFF00000 : 0) | (((uint32_t)buf[5] & 0x0F) << 16) | ((uint32_t)buf[6] << 8) | (uint32_t)buf[7];
_c01 = ((uint16_t)buf[8] << 8) | ((uint16_t)buf[9]);
_c11 = ((uint16_t)buf[10] << 8) | (uint16_t)buf[11];
_c20 = ((uint16_t)buf[12] << 8) | (uint16_t)buf[13];
_c21 = ((uint16_t)buf[14] << 8) | (uint16_t)buf[15];
_c30 = ((uint16_t)buf[16] << 8) | (uint16_t)buf[17];

#define READ_LENGTH 9

for (uint8_t i = 0; i < SPL06_CALIB_COEFFS_LEN; ) {
ssize_t chunk = MIN(READ_LENGTH, SPL06_CALIB_COEFFS_LEN - i);
if (!_dev->read_registers(SPL06_REG_CALIB_COEFFS_START + i, buf + i, chunk)) {
return false;
}
i += chunk;
}

// 0x11 c0 [3:0] + 0x10 c0 [11:4]
_c0 = get_twos_complement(((uint32_t)buf[0] << 4) | (((uint32_t)buf[1] >> 4) & 0x0F), 12);
// 0x11 c1 [11:8] + 0x12 c1 [7:0]
_c1 = get_twos_complement((((uint32_t)buf[1] & 0x0F) << 8) | (uint32_t)buf[2], 12);
// 0x13 c00 [19:12] + 0x14 c00 [11:4] + 0x15 c00 [3:0]
_c00 = get_twos_complement(((uint32_t)buf[3] << 12) | ((uint32_t)buf[4] << 4) | (((uint32_t)buf[5] >> 4) & 0x0F), 20);
// 0x15 c10 [19:16] + 0x16 c10 [15:8] + 0x17 c10 [7:0]
_c10 = get_twos_complement((((uint32_t)buf[5] & 0x0F) << 16) | ((uint32_t)buf[6] << 8) | (uint32_t)buf[7], 20);
// 0x18 c01 [15:8] + 0x19 c01 [7:0]
_c01 = get_twos_complement(((uint32_t)buf[8] << 8) | (uint32_t)buf[9], 16);
// 0x1A c11 [15:8] + 0x1B c11 [7:0]
_c11 = get_twos_complement(((uint32_t)buf[10] << 8) | (uint32_t)buf[11], 16);
// 0x1C c20 [15:8] + 0x1D c20 [7:0]
_c20 = get_twos_complement(((uint32_t)buf[12] << 8) | (uint32_t)buf[13], 16);
// 0x1E c21 [15:8] + 0x1F c21 [7:0]
_c21 = get_twos_complement(((uint32_t)buf[14] << 8) | (uint32_t)buf[15], 16);
// 0x20 c30 [15:8] + 0x21 c30 [7:0]
_c30 = get_twos_complement(((uint32_t)buf[16] << 8) | (uint32_t)buf[17], 16);

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];
// 0x23 c31 [3:0] + 0x22 c31 [11:4]
_c31 = get_twos_complement(((uint32_t)buf[18] << 4) | (((uint32_t)buf[19] >> 4) & 0x0F), 12);
// 0x23 c40 [11:8] + 0x24 c40 [7:0]
_c40 = get_twos_complement((((uint32_t)buf[19] & 0x0F) << 8) | (uint32_t)buf[20], 12);
}

// setup temperature and pressure measurements
_dev->setup_checked_registers(3, 20);

#if AP_BARO_SPL06_BACKGROUND_ENABLE
//set rate and oversampling
_dev->write_register(SPL06_REG_TEMPERATURE_CFG, SPL06_TEMP_RATE_32HZ | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_TEMPERATURE_OVERSAMPLING), true);
_dev->write_register(SPL06_REG_TEMPERATURE_CFG, SPL06_TEMP_USE_EXT_SENSOR | SPL06_TEMP_RATE_32HZ | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_TEMPERATURE_OVERSAMPLING), true);
_dev->write_register(SPL06_REG_PRESSURE_CFG, SPL06_PRES_RATE_32HZ | SPL06_OVERSAMPLING_TO_REG_VALUE(SPL06_PRESSURE_OVERSAMPLING), true);

//enable background mode
Expand Down
Loading