fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 11:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Jake Dahl
2018-10-16 15:42:09 -06:00
committed by Kabir Mohammed
parent 6a7f3615a4
commit 51ffee8137
1 changed files with 93 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/distance_sensor/pga460/pga460.cpp
+93 -96
View File
@@ -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);
px4_pollfd_struct_t fds[1];
fds[0].fd = _fd;
fds[0].events = POLLIN;
uint8_t buf_rx[4] = {};
int bytes_available = sizeof(buf_rx);
int total_bytes = 0;
int bytesread = 0;
int timeout = 10; // Wait up to 10ms inbetween bytes.
uint8_t buf_rx[4] = {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])), timeout);
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 err1: %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);
// 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.
usleep(10000);
if (ret < 0) {
PX4_WARN("px4_poll() failed.");
return PX4_ERROR;
}
int bytes_available = sizeof(buf_rx);
int total_bytes = 0;
while (bytes_read < sizeof(buf_rx)) {
if (fds[0].revents & POLLIN) {
do {
ret = px4_read(_fd, buf_rx + total_bytes, sizeof(buf_rx));
read_result = px4_read(_fd, buf_rx + bytes_read, sizeof(buf_rx) - bytes_read);
total_bytes += ret;
if (read_result >= 0) {
bytes_read += read_result;
} else {
return PX4_ERROR;
}
} 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");