fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 22:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

implemented retrying the connection to the px4flow sensor before giving up.

Browse Source
This commit is contained in:
Simon Laube
2015-06-30 18:28:19 +02:00
parent 93dfc435a4
commit 7a93348340
1 changed files with 76 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4flow/px4flow.cpp
+76 -57
View File
@@ -636,7 +636,11 @@ namespace px4flow
#endif
const int ERROR = -1;
PX4FLOW *g_dev;
PX4FLOW *g_dev = nullptr;
bool start_in_progress = false;
const int START_RETRY_COUNT = 5;
const int START_RETRY_TIMEOUT = 1000;
void start();
void stop();
@@ -651,78 +655,93 @@ void
start()
{
int fd;
/* entry check: */
if (start_in_progress) {
errx(1, "start in progress");
}
start_in_progress = true;
if (g_dev != nullptr) {
start_in_progress = false;
errx(1, "already started");
}
const int busses_to_try[] = {
PX4_I2C_BUS_EXPANSION,
#ifdef PX4_I2C_BUS_ESC
PX4_I2C_BUS_ESC,
#endif
PX4_I2C_BUS_ONBOARD,
-1
};
int retry_nr = 0;
while (1) {
const int busses_to_try[] = {
PX4_I2C_BUS_EXPANSION,
#ifdef PX4_I2C_BUS_ESC
PX4_I2C_BUS_ESC,
#endif
PX4_I2C_BUS_ONBOARD,
-1
};
const int *cur_bus = busses_to_try;
while(*cur_bus != -1) {
/* create the driver */
//warnx("trying bus %d", *cur_bus);
g_dev = new PX4FLOW(*cur_bus);
if (g_dev == nullptr) {
/* this is a fatal error */
break;
const int *cur_bus = busses_to_try;
while(*cur_bus != -1) {
/* create the driver */
/* warnx("trying bus %d", *cur_bus); */
g_dev = new PX4FLOW(*cur_bus);
if (g_dev == nullptr) {
/* this is a fatal error */
break;
}
/* init the driver: */
if (OK == g_dev->init()) {
/* success! */
break;
}
/* destroy it again because it failed. */
delete g_dev;
g_dev = nullptr;
/* try next! */
cur_bus++;
}
/* init the driver: */
if (OK == g_dev->init()) {
/* check whether we found it: */
if (*cur_bus != -1) {
/* check for failure: */
if (g_dev == nullptr) {
break;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
break;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
break;
}
/* success! */
start_in_progress = false;
exit(0);
}
if (retry_nr < START_RETRY_COUNT) {
warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr);
usleep(START_RETRY_TIMEOUT * 1000);
retry_nr++;
} else {
break;
}
/* destroy it again because it failed. */
delete g_dev;
g_dev = nullptr;
/* try next! */
cur_bus++;
}
/* check whether we found it: */
if (*cur_bus == -1) {
goto not_found;
}
/* check for failure: */
if (g_dev == nullptr) {
goto fatal_fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
goto fatal_fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
goto fatal_fail;
}
exit(0);
not_found:
/* for now we do the same as if there was a fatal failure. */
warnx("PX4FLOW not found on I2C busses");
fatal_fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
start_in_progress = false;
errx(1, "PX4FLOW could not be started over I2C");
}