aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Airspeed: fixed timing errors in MS5525 driver

Browse Source 
this fixes issue #7188, where we could read from the MS5525 sensor too
soon after requesting data and the result coming back would be garbage
This commit is contained in:
Andrew Tridgell 2017-12-21 15:04:13 +11:00 committed by Tom Pittenger
parent 6c6515711c
commit 55f73f4163
2 changed files with 37 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp
View File

@ -90,6 +90,7 @@ bool AP_Airspeed_MS5525::init()
uint8_t reg = REG_CONVERT_TEMPERATURE;
dev->transfer(&reg, 1, nullptr, 0);
state = 0;
command_send_us = AP_HAL::micros();
// drop to 2 retries for runtime
dev->set_retries(2);
@ -226,30 +227,54 @@ void AP_Airspeed_MS5525::calculate(void)
// 50Hz timer
void AP_Airspeed_MS5525::timer()
{
if (AP_HAL::micros() - command_send_us < 10000) {
// we should avoid trying to read the ADC too soon after
// sending the command
return;
}
uint32_t adc_val = read_adc();
if (adc_val == 0) {
// we have either done a read too soon after sending the
// request or we have tried to read the same sample twice. We
// re-send the command now as we don't know what state the
// sensor is in, and we do want to trigger it sending a value,
// which we will discard
if (dev->transfer(&cmd_sent, 1, nullptr, 0)) {
command_send_us = AP_HAL::micros();
}
// when we get adc_val == 0 then then both the current value and
// the next value we read from the sensor are invalid
ignore_next = true;
return;
}
/*
* If read fails, re-initiate a read command for current state or we are
* stuck
*/
uint8_t next_state = state;
if (adc_val != 0) {
next_state = (state + 1) % 5;
if (state == 0) {
if (!ignore_next) {
if (cmd_sent == REG_CONVERT_TEMPERATURE) {
D2 = adc_val;
} else {
} else if (cmd_sent == REG_CONVERT_PRESSURE) {
D1 = adc_val;
calculate();
}
}
uint8_t next_cmd = next_state == 0 ? REG_CONVERT_TEMPERATURE : REG_CONVERT_PRESSURE;
if (!dev->transfer(&next_cmd, 1, nullptr, 0)) {
ignore_next = false;
cmd_sent = (state == 0) ? REG_CONVERT_TEMPERATURE : REG_CONVERT_PRESSURE;
if (!dev->transfer(&cmd_sent, 1, nullptr, 0)) {
// we don't know for sure what state the sensor is in when we
// fail to send the command, so ignore the next response
ignore_next = true;
return;
}
command_send_us = AP_HAL::micros();
state = next_state;
state = (state + 1) % 5;
}
// return the current differential_pressure in Pascal

3
libraries/AP_Airspeed/AP_Airspeed_MS5525.h
View File

@ -64,6 +64,9 @@ private:
uint8_t state;
int32_t D1;
int32_t D2;
uint32_t command_send_us;
bool ignore_next;
uint8_t cmd_sent;
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
};