From 93dfc435a4b40f7b8eac541d902657ec881ff033 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 17:53:19 +0200 Subject: [PATCH 1/3] change the nested if structure which tries all i2c busses to a loop. --- src/drivers/px4flow/px4flow.cpp | 81 ++++++++++++++++++--------------- 1 file changed, 45 insertions(+), 36 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 0704f16c91e..ba438e276e0 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -656,65 +656,74 @@ start() errx(1, "already started"); } - /* create the driver */ - g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION); - - if (g_dev == nullptr) { - goto fail; - } - - if (OK != g_dev->init()) { - + const int busses_to_try[] = { + PX4_I2C_BUS_EXPANSION, #ifdef PX4_I2C_BUS_ESC - delete g_dev; - /* try 2nd bus */ - g_dev = new PX4FLOW(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) { - goto fail; + /* this is a fatal error */ + break; } - - if (OK != g_dev->init()) { - #endif - - delete g_dev; - /* 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 + + /* init the driver: */ + if (OK == g_dev->init()) { + /* success! */ + break; } - #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 */ fd = open(PX4FLOW0_DEVICE_PATH, O_RDONLY); if (fd < 0) { - goto fail; + goto fatal_fail; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) { - goto fail; + goto fatal_fail; } 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) { delete g_dev; g_dev = nullptr; } - errx(1, "no PX4FLOW connected over I2C"); + errx(1, "PX4FLOW could not be started over I2C"); } /** From 7a933483405962291eb7706c7de169ce50223955 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 18:28:19 +0200 Subject: [PATCH 2/3] implemented retrying the connection to the px4flow sensor before giving up. --- src/drivers/px4flow/px4flow.cpp | 133 ++++++++++++++++++-------------- 1 file changed, 76 insertions(+), 57 deletions(-) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index ba438e276e0..8d0af42524e 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -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"); } From 07efb655c4da5c0fcaa7b99a3524743f94169f7e Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 21:10:48 +0200 Subject: [PATCH 3/3] change start script to launch the px4flow driver in background. Fixes issue #2145 --- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 536cfca91ac..d9cdfa5dc0b 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -111,7 +111,7 @@ else fi # Check for flow sensor -if px4flow start +if px4flow start & then fi