dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

change the nested if structure which tries all i2c busses to a loop.

This commit is contained in:
Simon Laube 2015-06-30 17:53:19 +02:00
parent a1dd0bc758
commit 93dfc435a4
1 changed files with 45 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
src/drivers/px4flow/px4flow.cpp
View File

@ -656,65 +656,74 @@ start()
errx(1, "already started"); errx(1, "already started");
} }
/* create the driver */ const int busses_to_try[] = {
g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION); PX4_I2C_BUS_EXPANSION,
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
#ifdef PX4_I2C_BUS_ESC #ifdef PX4_I2C_BUS_ESC
delete g_dev; PX4_I2C_BUS_ESC,
/* try 2nd bus */ #endif
g_dev = new PX4FLOW(PX4_I2C_BUS_ESC); 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) { if (g_dev == nullptr) {
goto fail; /* this is a fatal error */
break;
} }
if (OK != g_dev->init()) { /* init the driver: */
#endif if (OK == g_dev->init()) {
/* success! */
delete g_dev; break;
/* try 3rd bus */
g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
#ifdef PX4_I2C_BUS_ESC
} }
#endif
/* 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 */ /* set the poll rate to default, starts automatic data collection */
fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY);
if (fd < 0) { if (fd < 0) {
goto fail; goto fatal_fail;
} }
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
goto fail; goto fatal_fail;
} }
exit(0); exit(0);
fail: 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) { if (g_dev != nullptr) {
delete g_dev; delete g_dev;
g_dev = nullptr; g_dev = nullptr;
} }
errx(1, "no PX4FLOW connected over I2C"); errx(1, "PX4FLOW could not be started over I2C");
} }
/** /**