// Read the sensor. This is a state machine
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
// temperature does not change so quickly...
void AP_Baro_MS5611::_update(uint32_t tnow)
{
if (_sync_access) return;
// Throttle read rate to 100hz maximum.
// note we use 9500us here not 10000us
// the read rate will end up at exactly 100hz because the Periodic Timer fires at 1khz
if (tnow - _timer < 9500) {
return;
}
_timer = tnow;
if (_state == 1) {
_s_D2 = _spi_read_adc(); // On state 1 we read temp
_state++;
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
} else if (_state == 5) {
_s_D1 = _spi_read_adc();
_state = 1; // Start again from state = 1
_spi_write(CMD_CONVERT_D2_OSR4096); // Command to read temperature
_updated = true; // New pressure reading
} else {
_s_D1 = _spi_read_adc();
_state++;
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
_updated = true; // New pressure reading
}
}
// Read the sensor. This is a state machine
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
// temperature does not change so quickly...
void AP_Baro_MS5611::_update(uint32_t tnow)
{
if (_sync_access) return;
if (tnow - _timer < 10000) {
return; // wait for more than 10ms
}
_timer = tnow;
if (_state == 1) {
_s_D2 = _spi_read_adc(); // On state 1 we read temp
_state++;
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
} else if (_state == 5) {
_s_D1 = _spi_read_adc();
_state = 1; // Start again from state = 1
_spi_write(CMD_CONVERT_D2_OSR4096); // Command to read temperature
_updated = true; // New pressure reading
} else {
_s_D1 = _spi_read_adc();
_state++;
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
_updated = true; // New pressure reading
}
}
// Read the sensor. This is a state machine
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
// temperature does not change so quickly...
void AP_Baro_MS5611::_update(uint32_t tnow)
{
// Throttle read rate to 100hz maximum.
// note we use 9500us here not 10000us
// the read rate will end up at exactly 100hz because the Periodic Timer fires at 1khz
if (tnow - _timer < 9500) {
return;
}
_timer = tnow;
if (_state == 0) {
_s_D2 += _spi_read_adc(); // On state 0 we read temp
_d2_count++;
if (_d2_count == 32) {
// we have summed 32 values. This only happens
// when we stop reading the barometer for a long time
// (more than 1.2 seconds)
_s_D2 >>= 1;
_d2_count = 16;
}
// Read the sensor. This is a state machine
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
// temperature does not change so quickly...
void AP_Baro_MS5611::_update(uint32_t tnow)
{
// Throttle read rate to 100hz maximum.
// note we use 9500us here not 10000us
// the read rate will end up at exactly 100hz because the Periodic Timer fires at 1khz
if (tnow - _timer < 9500) {
return;
}
static int semfail_ctr = 0;
if (_spi_sem) {
bool got = _spi_sem->get((void*)&_spi_sem);
if (!got) {
semfail_ctr++;
if (semfail_ctr > 100) {
hal.scheduler->panic(PSTR("PANIC: failed to take _spi_sem "
"100 times in AP_Baro_MS5611::_update"));
}
return;
} else {
semfail_ctr = 0;
}
}
_timer = tnow;
if (_state == 0) {
_s_D2 += _spi_read_adc();// On state 0 we read temp
_d2_count++;
if (_d2_count == 32) {
// we have summed 32 values. This only happens
// when we stop reading the barometer for a long time
// (more than 1.2 seconds)
_s_D2 >>= 1;
_d2_count = 16;
}
