removed px4_poll, something changed and broke the way I was using it. This has been tested successfully

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");