diff --git a/src/drivers/distance_sensor/pga460/pga460.cpp b/src/drivers/distance_sensor/pga460/pga460.cpp
index b7a1d7d915be89f9bcb76bdae2ad102279ef5f4a..bf179c3533cdd7b731e48775308128a72bb43c0d 100644
--- a/src/drivers/distance_sensor/pga460/pga460.cpp
+++ b/src/drivers/distance_sensor/pga460/pga460.cpp
@@ -118,14 +118,12 @@ int PGA460::initialize_device_settings()
// Read to see if eeprom saved data matches desired data, otherwise overwrite eeprom.
if (read_eeprom() != PX4_OK) {
write_eeprom();
+ // Allow sufficient time for the device to complete writing to registers.
+ usleep(10000);
}
- // Allow sufficient time for the device to complete writing to registers.
- usleep(10000);
-
// Verify the device is alive.
if (read_register(0x00) != USER_DATA1) {
- close_serial();
return PX4_ERROR;
}
@@ -152,34 +150,39 @@ int PGA460::initialize_thresholds()
// Must wait >50us per datasheet.
usleep(100);
- if (read_threshold_registers()) {
- return 1;
+ if (read_threshold_registers() == PX4_OK) {
+ return PX4_OK;
} else {
print_device_status();
- return 0;
+ return PX4_ERROR;
}
}
uint32_t PGA460::collect_results()
{
- px4_pollfd_struct_t fds[1];
- fds[0].fd = _fd;
- fds[0].events = POLLIN;
- int timeout = 10;
- uint8_t buf_rx[6] = {0};
+ uint8_t buf_rx[6] = {};
+ int bytes_available = sizeof(buf_rx);
+ int total_bytes = 0;
- int ret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), timeout);
+ do {
+ int ret = px4_read(_fd, buf_rx + total_bytes, sizeof(buf_rx));
- // Waiting for a maximum of 10ms.
- if (ret > 0) {
- usleep(10000);
- px4_read(_fd, buf_rx, sizeof(buf_rx));
+ total_bytes += ret;
+
+ if (ret < 0) {
+ tcflush(_fd, TCIFLUSH);
+ PX4_ERR("read err3: %d", ret);
+ return ret;
+ }
+
+ bytes_available -= ret;
+
+ usleep(1000);
+
+ } while (bytes_available > 0);
- } else {
- PX4_WARN("px4_poll() failed");
- }
uint16_t time_of_flight = (buf_rx[1] << 8) + buf_rx[2];
uint8_t Width = buf_rx[3];
@@ -233,29 +236,32 @@ float PGA460::get_temperature()
px4_write(_fd, &buf_tx[0], sizeof(buf_tx));
// The pga460 requires a 2ms delay per the datasheet.
- usleep(2000);
+ usleep(5000);
buf_tx[1] = TNLR;
px4_write(_fd, &buf_tx[0], sizeof(buf_tx) - 2);
+ usleep(10000);
+
+ uint8_t buf_rx[4] = {};
+ int bytes_available = sizeof(buf_rx);
+ int total_bytes = 0;
- px4_pollfd_struct_t fds[1];
- fds[0].fd = _fd;
- fds[0].events = POLLIN;
+ do {
+ int ret = px4_read(_fd, buf_rx + total_bytes, sizeof(buf_rx));
- int bytesread = 0;
- int timeout = 10; // Wait up to 10ms inbetween bytes.
- uint8_t buf_rx[4] = {0};
+ total_bytes += ret;
- int ret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), timeout);
+ if (ret < 0) {
+ tcflush(_fd, TCIFLUSH);
+ PX4_ERR("read err1: %d", ret);
+ return ret;
+ }
- while (ret) {
- if (fds[0].revents & POLLIN) {
- bytesread += px4_read(_fd, buf_rx + bytesread, sizeof(buf_rx) - bytesread);
+ bytes_available -= ret;
- } else { break; }
+ usleep(1000);
- ret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), timeout);
- }
+ } while (bytes_available > 0);
// These constants and equations are from the pga460 datasheet, page 50.
float juntion_to_ambient_thermal_resistance = 96.1;
@@ -482,7 +488,7 @@ int PGA460::read_eeprom()
unlock_eeprom();
const int array_size = 43;
- const uint8_t user_settings[array_size] =
+ uint8_t user_settings[array_size] =
{USER_DATA1, USER_DATA2, USER_DATA3, USER_DATA4,
USER_DATA5, USER_DATA6, USER_DATA7, USER_DATA8, USER_DATA9, USER_DATA10,
USER_DATA11, USER_DATA12, USER_DATA13, USER_DATA14, USER_DATA15, USER_DATA16,
@@ -491,18 +497,10 @@ int PGA460::read_eeprom()
PULSE_P1, PULSE_P2, CURR_LIM_P1, CURR_LIM_P2, REC_LENGTH, FREQ_DIAG, SAT_FDIAG_TH, FVOLT_DEC, DECPL_TEMP,
DSP_SCALE, TEMP_TRIM, P1_GAIN_CTRL, P2_GAIN_CTRL};
- px4_pollfd_struct_t fds[1];
- fds[0].fd = _fd;
- fds[0].events = POLLIN;
int ret = -1;
- int read_result = 0;
- int timeout = 100;
-
- size_t bytes_read = 0;
-
uint8_t cmd_buf[2] = {SYNCBYTE, EEBR};
- uint8_t buf_rx[array_size + 2] = {0};
+ uint8_t buf_rx[array_size + 2] = {};
// The pga460 responds to this write() call by reporting current eeprom values.
ret = px4_write(_fd, &cmd_buf[0], sizeof(cmd_buf));
@@ -511,45 +509,37 @@ int PGA460::read_eeprom()
PX4_WARN("px4_write() failed.");
}
- usleep(1000); // Sleep for 1ms to allow write to complete before polling.
- ret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), timeout);
- usleep(1000); // Sleep for 1ms to allow data to be received.
-
- if (ret < 0) {
- PX4_WARN("px4_poll() failed.");
- return PX4_ERROR;
- }
+ usleep(10000);
- while (bytes_read < sizeof(buf_rx)) {
- if (fds[0].revents & POLLIN) {
+ int bytes_available = sizeof(buf_rx);
+ int total_bytes = 0;
- read_result = px4_read(_fd, buf_rx + bytes_read, sizeof(buf_rx) - bytes_read);
+ do {
+ ret = px4_read(_fd, buf_rx + total_bytes, sizeof(buf_rx));
- if (read_result >= 0) {
- bytes_read += read_result;
- } else {
- return PX4_ERROR;
- }
+ total_bytes += ret;
- } else {
- break;
+ if(ret < 0) {
+ tcflush(_fd, TCIFLUSH);
+ PX4_ERR("read err2: %d", ret);
+ return ret;
}
- ret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), timeout);
+ bytes_available -= ret;
- if (ret < 0) {
- PX4_WARN("px4_poll() failed.");
- break;
- }
- }
+ usleep(1000);
+
+ } while (bytes_available > 0);
// Check the buffers to ensure they match.
int mismatched_bytes = memcmp(buf_rx + 1, user_settings, array_size);
if (mismatched_bytes == 0) {
+ PX4_INFO("EEPROM has settings.");
return PX4_OK;
} else {
print_diagnostics(buf_rx[0]);
+ PX4_INFO("EEPROM does not have settings.");
return PX4_ERROR;
}
}
@@ -566,25 +556,28 @@ uint8_t PGA460::read_register(const uint8_t reg)
buf_tx[3] = checksum;
px4_write(_fd, &buf_tx[0], sizeof(buf_tx));
+ usleep(10000);
- px4_pollfd_struct_t fds[1];
- fds[0].fd = _fd;
- fds[0].events = POLLIN;
+ uint8_t buf_rx[3] = {};
+ int bytes_available = sizeof(buf_rx);
+ int total_bytes = 0;
- int timeout = 100;
- int bytesread = 0;
- uint8_t buf_rx[3] = {0};
+ do {
+ int ret = px4_read(_fd, buf_rx + total_bytes, sizeof(buf_rx));
- int ret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+ total_bytes += ret;
- while (ret) {
- if (fds[0].revents & POLLIN) {
- bytesread += px4_read(_fd, buf_rx + bytesread, sizeof(buf_rx) - bytesread);
+ if(ret < 0) {
+ tcflush(_fd, TCIFLUSH);
+ PX4_ERR("read err3: %d", ret);
+ return ret;
+ }
- } else { break; }
+ bytes_available -= ret;
- ret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), timeout);
- }
+ usleep(1000);
+
+ } while (bytes_available > 0);
// Prints errors if there are any.
print_diagnostics(buf_rx[0]);
@@ -606,25 +599,28 @@ int PGA460::read_threshold_registers()
uint8_t buf_tx[2] = {SYNCBYTE, THRBR};
px4_write(_fd, &buf_tx[0], sizeof(buf_tx));
+ usleep(10000);
- px4_pollfd_struct_t fds[1];
- fds[0].fd = _fd;
- fds[0].events = POLLIN;
+ uint8_t buf_rx[array_size + 2] = {};
+ int bytes_available = sizeof(buf_rx);
+ int total_bytes = 0;
- int timeout = 100;
- int bytesread = 0;
- uint8_t buf_rx[array_size + 2] = {0};
+ do {
+ int ret = px4_read(_fd, buf_rx + total_bytes, sizeof(buf_rx));
- int ret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+ total_bytes += ret;
- while (ret) {
- if (fds[0].revents & POLLIN) {
- bytesread += px4_read(_fd, buf_rx + bytesread, sizeof(buf_rx) - bytesread);
+ if(ret < 0) {
+ tcflush(_fd, TCIFLUSH);
+ PX4_ERR("read err3: %d", ret);
+ return ret;
+ }
- } else { break; }
+ bytes_available -= ret;
- ret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), timeout);
- }
+ usleep(1000);
+
+ } while (bytes_available > 0);
// Check to ensure the buffers match.
int mismatch = memcmp(buf_rx + 1, user_settings, sizeof(buf_rx) - 2);
@@ -644,6 +640,7 @@ void PGA460::request_results()
{
uint8_t buf_tx[2] = {SYNCBYTE, UMR};
px4_write(_fd, &buf_tx[0], sizeof(buf_tx));
+ usleep(10000);
}
void PGA460::run()
@@ -661,7 +658,7 @@ void PGA460::run()
_distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &report);
if (_distance_sensor_topic == nullptr) {
- PX4_WARN("Failed to advertise distance_sensor topic. Did you start uORB?");
+ PX4_WARN("Advertise failed.");
return;
}
@@ -818,7 +815,7 @@ int PGA460::write_eeprom()
// Give up to 100ms for ee_cntrl register to reflect a successful eeprom write.
for (int i = 0; i < 100; i++) {
result = read_register(EE_CNTRL_ADDR);
- usleep(1000);
+ usleep(5000);
if (result & 1 << 2) {
PX4_INFO("EEPROM write successful");