From dac74db104f8fda5bd3b8b68f3b7f45878a27865 Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 17:53:19 +0200 Subject: [PATCH 1/4] 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 2166258c15..8bf20290ca 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -616,65 +616,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 d9e6cb0f584f6e4016688a12a03e641af47cff4a Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 18:28:19 +0200 Subject: [PATCH 2/4] 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 8bf20290ca..8ee365d8df 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -596,7 +596,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(); @@ -611,78 +615,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 440aedebadc3db8ee750736ab297b25a4c04ec4a Mon Sep 17 00:00:00 2001 From: Simon Laube Date: Tue, 30 Jun 2015 21:10:48 +0200 Subject: [PATCH 3/4] 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 474db36ef7..e5d527dd71 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 From cf8307f039f328a42afc719fe1a74492c7edb527 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 18:57:59 +0200 Subject: [PATCH 4/4] Commander: Low-pass battery throttle to better match battery dynamics --- src/modules/commander/commander_helper.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 68949eec1d..491ce32a5c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -108,6 +108,7 @@ static float bat_v_load_drop = 0.06f; static int bat_n_cells = 3; static float bat_capacity = -1.0f; static unsigned int counter = 0; +static float throttle_lowpassed = 0.0f; int battery_init() { @@ -381,8 +382,15 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float counter++; + // XXX this time constant needs to become tunable + // but really, the right fix are smart batteries. + float val = throttle_lowpassed * 0.97f + throttle_normalized * 0.03f; + if (isfinite(val)) { + throttle_lowpassed = val; + } + /* remaining charge estimate based on voltage and internal resistance (drop under load) */ - float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_normalized); + float bat_v_empty_dynamic = bat_v_empty - (bat_v_load_drop * throttle_lowpassed); /* the range from full to empty is the same for batteries under load and without load, * since the voltage drop applies to both the full and empty state */