From c025f9863fb6e7cfd53a13dc74b086d19188b57a Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Sat, 5 Jul 2014 03:22:23 -0400 Subject: [PATCH 01/17] Added and registered new module for PX4Flow LED assistance lighting. --- makefiles/config_px4fmu-v2_default.mk | 1 + src/modules/flow_led/flow_led.c | 105 ++++++++++++++++++++++++++ src/modules/flow_led/module.mk | 2 + 3 files changed, 108 insertions(+) create mode 100644 src/modules/flow_led/flow_led.c create mode 100644 src/modules/flow_led/module.mk diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a68cc32f47d0..8f4b2a1f2c87 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -74,6 +74,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led +MODULES += modules/flow_led # # Estimation modules (EKF/ SO3 / other filters) diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c new file mode 100644 index 000000000000..e6693cd89077 --- /dev/null +++ b/src/modules/flow_led/flow_led.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Antonio Sanniravong + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file flow_led.c + * PX4Flow assistive LED lighting control. + */ + +#include +#include +#include +#include +#include +#include + +__EXPORT int flow_led_main(int argc, char *argv[]); + +int flow_led_main(int argc, char *argv[]) +{ + /* Initialize structs */ + struct optical_flow_s flow; + memset(&flow, 0, sizeof(flow)); + + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + + /* Subscribe to flow topic */ + int flow_sub = orb_subscribe(ORB_ID(optical_flow)); + + /* Advertise on actuators topic */ + orb_advert_t actuators_pub = orb_advertise(ORB_ID(actuator_outputs), &actuators); + + struct pollfd fds[] = { + { .fd = flow_sub, .events = POLLIN } + }; + + int error_counter = 0; + int flow_q = 0; + float sonar = 0.0f; + float sonar_avg = 0.0f; + float led_out = 0.0f; + + while (1) { + /* wait for update for 1000 ms */ + int poll_result = poll(fds, 1, 1000); + + if (poll_result == 0) { + /* No new flow data */ + printf("[flow_led] Got no data within a second\n"); + + } else if (poll_result < 0) { + /* ERROR */ + if (error_counter < 10 || error_counter % 50 == 0) { + /* Use error counter to prevent flooding */ + printf("[flow_led] ERROR return value from poll(): %d\n", poll_result); + } + error_counter++; + + } else { + + if (fds[0].revents & POLLIN) { + /* Flow data obtained */ + + /* Copy flow data to local buffer */ + orb_copy(ORB_ID(optical_flow), flow_sub, &flow); + flow_q = flow.quality; + sonar = flow.ground_distance_m; + + } + } + } + + return 0; +} \ No newline at end of file diff --git a/src/modules/flow_led/module.mk b/src/modules/flow_led/module.mk new file mode 100644 index 000000000000..0ffde2b18f4c --- /dev/null +++ b/src/modules/flow_led/module.mk @@ -0,0 +1,2 @@ +MODULE_COMMAND = flow_led +SRCS = flow_led.c \ No newline at end of file From 85a0188ebb47ef26cf10c63eac01ea10e60bec2b Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Mon, 14 Jul 2014 04:07:58 -0400 Subject: [PATCH 02/17] Allow lower min and higher max PWM outputs. --- src/drivers/drv_pwm_output.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 972573f9ff2b..6fdb09e3e0ea 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -67,7 +67,7 @@ __BEGIN_DECLS /** * Lowest minimum PWM in us */ -#define PWM_LOWEST_MIN 900 +#define PWM_LOWEST_MIN 100 /** * Default minimum PWM in us @@ -82,7 +82,7 @@ __BEGIN_DECLS /** * Highest maximum PWM in us */ -#define PWM_HIGHEST_MAX 2100 +#define PWM_HIGHEST_MAX 20000 /** * Default maximum PWM in us From e12082a2f41adcb6d943928d6d60c1986bfe5bea Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Mon, 14 Jul 2014 19:12:37 -0400 Subject: [PATCH 03/17] Added LED blinking example code. --- src/modules/flow_led/flow_led.c | 47 +++++++++++++++++++++++++++++---- 1 file changed, 42 insertions(+), 5 deletions(-) diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c index e6693cd89077..845c6ae7eb81 100644 --- a/src/modules/flow_led/flow_led.c +++ b/src/modules/flow_led/flow_led.c @@ -43,6 +43,8 @@ #include #include #include +#include +#include __EXPORT int flow_led_main(int argc, char *argv[]); @@ -59,7 +61,7 @@ int flow_led_main(int argc, char *argv[]) int flow_sub = orb_subscribe(ORB_ID(optical_flow)); /* Advertise on actuators topic */ - orb_advert_t actuators_pub = orb_advertise(ORB_ID(actuator_outputs), &actuators); + orb_advert_t actuators_pub = orb_advertise(ORB_ID(actuator_outputs_1), &actuators); struct pollfd fds[] = { { .fd = flow_sub, .events = POLLIN } @@ -70,10 +72,17 @@ int flow_led_main(int argc, char *argv[]) float sonar = 0.0f; float sonar_avg = 0.0f; float led_out = 0.0f; + hrt_abstime t_prev = 0; + char down = 0; while (1) { - /* wait for update for 1000 ms */ - int poll_result = poll(fds, 1, 1000); + /* wait for update for 500 ms */ + int poll_result = poll(fds, 1, 500); + hrt_abstime t = hrt_absolute_time(); + + float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; + dt = fmaxf(fminf(0.5, dt), 0.05); // Constrain dt from 2 to 500 ms + t_prev = t; if (poll_result == 0) { /* No new flow data */ @@ -96,10 +105,38 @@ int flow_led_main(int argc, char *argv[]) orb_copy(ORB_ID(optical_flow), flow_sub, &flow); flow_q = flow.quality; sonar = flow.ground_distance_m; - } } + + /* Update LED output PWM value */ + /* TEMP: Gradually blink LED */ + if (!down && led_out == 1.0f){ + down = 1; + } else if (down && led_out == 0.0f) { + down = 0; + } + + if (!down && led_out < 1.0f) { + led_out += (1 - led_out) * dt * 0.5f; + } else if (down && led_out > 0.0f) { + led_out += - led_out * dt * 0.5f; + } + + /* Limit LED output */ + if (led_out <= 1.0f){ + led_out = 1.0f; + } else if (led_out <= 0.0f) { + led_out = 0.0f; + } + + /* Copy controls and timestamp to struct */ + actuators.control = led_out; + actuators.timestamp = t; + + /* Publish PWM output */ + orb_publish(ORB_ID(actuator_outputs_1), actuators_pub, &actuators); + } return 0; -} \ No newline at end of file +} From 45029278ffb716c9f563c9fffaa30bc279b9d555 Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Mon, 14 Jul 2014 20:59:15 -0400 Subject: [PATCH 04/17] Add flow_led module to px4fmu-v1 config. --- makefiles/config_px4fmu-v1_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 810e008aeb21..521754d977ee 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -65,6 +65,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led +MODULES += modules/flow_led # # Estimation modules (EKF/ SO3 / other filters) From 372c9e2bee45571f138e658fd6b3a81137218f8a Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Mon, 14 Jul 2014 21:56:18 -0400 Subject: [PATCH 05/17] Add string.h to includes for memset. --- src/modules/flow_led/flow_led.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c index 845c6ae7eb81..b78db76f42de 100644 --- a/src/modules/flow_led/flow_led.c +++ b/src/modules/flow_led/flow_led.c @@ -39,6 +39,7 @@ #include #include +#include #include #include #include From 05f754e334cb09efbb4da780bb042c370c13c3b1 Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Mon, 14 Jul 2014 22:10:08 -0400 Subject: [PATCH 06/17] Fix use of actuator controls instead of actuator outputs. --- src/modules/flow_led/flow_led.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c index b78db76f42de..766a4a43f833 100644 --- a/src/modules/flow_led/flow_led.c +++ b/src/modules/flow_led/flow_led.c @@ -44,7 +44,7 @@ #include #include #include -#include +#include #include __EXPORT int flow_led_main(int argc, char *argv[]); @@ -62,7 +62,7 @@ int flow_led_main(int argc, char *argv[]) int flow_sub = orb_subscribe(ORB_ID(optical_flow)); /* Advertise on actuators topic */ - orb_advert_t actuators_pub = orb_advertise(ORB_ID(actuator_outputs_1), &actuators); + orb_advert_t actuators_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); struct pollfd fds[] = { { .fd = flow_sub, .events = POLLIN } @@ -131,11 +131,11 @@ int flow_led_main(int argc, char *argv[]) } /* Copy controls and timestamp to struct */ - actuators.control = led_out; + actuators.control[0] = led_out; actuators.timestamp = t; /* Publish PWM output */ - orb_publish(ORB_ID(actuator_outputs_1), actuators_pub, &actuators); + orb_publish(ORB_ID(actuator_controls_1), actuators_pub, &actuators); } From dfe1cab2e8809539a3cbdafac6b7484c46bd1c2a Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Mon, 14 Jul 2014 23:31:18 -0400 Subject: [PATCH 07/17] Moved main to a thread. --- src/modules/flow_led/flow_led.c | 93 +++++++++++++++++++++++++++++---- 1 file changed, 83 insertions(+), 10 deletions(-) diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c index 766a4a43f833..628c50addf6a 100644 --- a/src/modules/flow_led/flow_led.c +++ b/src/modules/flow_led/flow_led.c @@ -37,20 +37,92 @@ * PX4Flow assistive LED lighting control. */ -#include +#include +#include #include +#include +#include #include +#include +#include +#include +#include #include +#include #include #include #include #include +#include +#include +#include #include __EXPORT int flow_led_main(int argc, char *argv[]); +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int flow_led_task; /**< Handle of deamon task / thread */ +static bool verbose_mode = false; + int flow_led_main(int argc, char *argv[]) { + if (argc < 1) { + usage("missing command"); + } + + if (!strcmp(argv[1], "start")) { + if (thread_running) { + warnx("already running"); + /* this is not an error */ + exit(0); + } + + verbose_mode = false; + + if (argc > 1) + if (!strcmp(argv[2], "-v")) { + verbose_mode = true; + } + + thread_should_exit = false; + flow_led_task = task_spawn_cmd("flow_led", + SCHED_DEFAULT, SCHED_PRIORITY_MAX - 50, 5000, + flow_led_thread_main, + (argv) ? (const char **) &argv[2] : (const char **) NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + if (thread_running) { + warnx("stop"); + thread_should_exit = true; + + } else { + warnx("app not started"); + } + + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("app is running"); + + } else { + warnx("app not started"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int flow_led_thread_main(int argc, char *argv[]) +{ + thread_running = true; /* Initialize structs */ struct optical_flow_s flow; memset(&flow, 0, sizeof(flow)); @@ -74,9 +146,9 @@ int flow_led_main(int argc, char *argv[]) float sonar_avg = 0.0f; float led_out = 0.0f; hrt_abstime t_prev = 0; - char down = 0; + char go_down = 0; - while (1) { + while (!thread_should_exit) { /* wait for update for 500 ms */ int poll_result = poll(fds, 1, 500); hrt_abstime t = hrt_absolute_time(); @@ -111,15 +183,15 @@ int flow_led_main(int argc, char *argv[]) /* Update LED output PWM value */ /* TEMP: Gradually blink LED */ - if (!down && led_out == 1.0f){ - down = 1; - } else if (down && led_out == 0.0f) { - down = 0; + if (!go_down && led_out == 1.0f){ + go_down = 1; + } else if (go_down && led_out == 0.0f) { + go_down = 0; } - if (!down && led_out < 1.0f) { + if (!go_down && led_out < 1.0f) { led_out += (1 - led_out) * dt * 0.5f; - } else if (down && led_out > 0.0f) { + } else if (go_down && led_out > 0.0f) { led_out += - led_out * dt * 0.5f; } @@ -138,6 +210,7 @@ int flow_led_main(int argc, char *argv[]) orb_publish(ORB_ID(actuator_controls_1), actuators_pub, &actuators); } - + warnx("stopped"); + thread_running = false; return 0; } From 027dc212156ddd305e8dbdb4a1808f977301aacb Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Mon, 14 Jul 2014 23:57:15 -0400 Subject: [PATCH 08/17] Added usage() function definition. --- src/modules/flow_led/flow_led.c | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c index 628c50addf6a..75369a19f225 100644 --- a/src/modules/flow_led/flow_led.c +++ b/src/modules/flow_led/flow_led.c @@ -60,11 +60,25 @@ __EXPORT int flow_led_main(int argc, char *argv[]); +int flow_led_thread_main(int argc, char *argv[]); + +static void usage(const char *reason); + static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int flow_led_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; +static void usage(const char *reason) +{ + if (reason) { + fprintf(stderr, "%s\n", reason); + } + + fprintf(stderr, "usage: flow_led {start|stop|status} [-v]\n\n"); + exit(1); +} + int flow_led_main(int argc, char *argv[]) { if (argc < 1) { From daa741b1fd09e237eadd32bc514013b24b22479e Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 15 Jul 2014 00:58:06 -0400 Subject: [PATCH 09/17] Mute printf's and advertise for actuator_controls_1. --- src/modules/flow_led/flow_led.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c index 75369a19f225..e46c64d0c6db 100644 --- a/src/modules/flow_led/flow_led.c +++ b/src/modules/flow_led/flow_led.c @@ -173,13 +173,13 @@ int flow_led_thread_main(int argc, char *argv[]) if (poll_result == 0) { /* No new flow data */ - printf("[flow_led] Got no data within a second\n"); + // printf("[flow_led] Got no data within a second\n"); } else if (poll_result < 0) { /* ERROR */ if (error_counter < 10 || error_counter % 50 == 0) { /* Use error counter to prevent flooding */ - printf("[flow_led] ERROR return value from poll(): %d\n", poll_result); + // printf("[flow_led] ERROR return value from poll(): %d\n", poll_result); } error_counter++; @@ -221,7 +221,11 @@ int flow_led_thread_main(int argc, char *argv[]) actuators.timestamp = t; /* Publish PWM output */ - orb_publish(ORB_ID(actuator_controls_1), actuators_pub, &actuators); + if (actuators_pub < 0) { + actuators_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); + } else { + orb_publish(ORB_ID(actuator_controls_1), actuators_pub, &actuators); + } } warnx("stopped"); From cc3d5c08b1f2f965245d250a5e814c60e753124e Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 15 Jul 2014 01:58:24 -0400 Subject: [PATCH 10/17] Fix error on PWM out limitting. --- src/modules/flow_led/flow_led.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c index e46c64d0c6db..77d81f4aed5f 100644 --- a/src/modules/flow_led/flow_led.c +++ b/src/modules/flow_led/flow_led.c @@ -210,9 +210,9 @@ int flow_led_thread_main(int argc, char *argv[]) } /* Limit LED output */ - if (led_out <= 1.0f){ + if (led_out > 1.0f){ led_out = 1.0f; - } else if (led_out <= 0.0f) { + } else if (led_out < 0.0f) { led_out = 0.0f; } From 85395d902e1a9eee24e6096e48f29646a6991e3b Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 15 Jul 2014 02:26:52 -0400 Subject: [PATCH 11/17] Added some debugging messages and modified LED output. --- src/modules/flow_led/flow_led.c | 48 ++++++++++++++++++--------------- 1 file changed, 26 insertions(+), 22 deletions(-) diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c index 77d81f4aed5f..d90e913dbfcd 100644 --- a/src/modules/flow_led/flow_led.c +++ b/src/modules/flow_led/flow_led.c @@ -134,13 +134,13 @@ int flow_led_main(int argc, char *argv[]) exit(1); } -int flow_led_thread_main(int argc, char *argv[]) -{ +int flow_led_thread_main(int argc, char *argv[]) { + thread_running = true; + /* Initialize structs */ struct optical_flow_s flow; memset(&flow, 0, sizeof(flow)); - struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); @@ -150,30 +150,39 @@ int flow_led_thread_main(int argc, char *argv[]) /* Advertise on actuators topic */ orb_advert_t actuators_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); + /* Initialize MAVLink fd for output to QGC */ + int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + // mavlink_log_info(mavlink_fd, "[inav] started"); + + /* Polling fds */ struct pollfd fds[] = { { .fd = flow_sub, .events = POLLIN } }; + /* Variable initializations */ int error_counter = 0; + int loop_counter = 0; int flow_q = 0; float sonar = 0.0f; float sonar_avg = 0.0f; float led_out = 0.0f; hrt_abstime t_prev = 0; - char go_down = 0; while (!thread_should_exit) { - /* wait for update for 500 ms */ + + /* Wait for update for 500 ms */ int poll_result = poll(fds, 1, 500); hrt_abstime t = hrt_absolute_time(); + /* Calculate time difference since last iteration of loop */ float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; dt = fmaxf(fminf(0.5, dt), 0.05); // Constrain dt from 2 to 500 ms t_prev = t; if (poll_result == 0) { /* No new flow data */ - // printf("[flow_led] Got no data within a second\n"); + // printf("[flow_led] Got no data within 500ms \n"); } else if (poll_result < 0) { /* ERROR */ @@ -192,21 +201,18 @@ int flow_led_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(optical_flow), flow_sub, &flow); flow_q = flow.quality; sonar = flow.ground_distance_m; + + // TODO: Update led_out with respect to flow quality and sonar height } } /* Update LED output PWM value */ - /* TEMP: Gradually blink LED */ - if (!go_down && led_out == 1.0f){ - go_down = 1; - } else if (go_down && led_out == 0.0f) { - go_down = 0; - } - - if (!go_down && led_out < 1.0f) { - led_out += (1 - led_out) * dt * 0.5f; - } else if (go_down && led_out > 0.0f) { - led_out += - led_out * dt * 0.5f; + /* TEMP: Blink LED */ + if (led_out < 0.5f && !(loop_counter % 10)){ + led_out = 1.0f; + mavlink_log_info(mavlink_fd, "Led output: %.2f", led_out) + } else if (led_out < 0.5f && !(loop_counter % 10)) { + led_out = 0.0f; } /* Limit LED output */ @@ -221,11 +227,9 @@ int flow_led_thread_main(int argc, char *argv[]) actuators.timestamp = t; /* Publish PWM output */ - if (actuators_pub < 0) { - actuators_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); - } else { - orb_publish(ORB_ID(actuator_controls_1), actuators_pub, &actuators); - } + orb_publish(ORB_ID(actuator_controls_1), actuators_pub, &actuators); + + loop_counter++; } warnx("stopped"); From 9d4e6286c55770c6e278694bf4913d865714f818 Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 15 Jul 2014 02:39:31 -0400 Subject: [PATCH 12/17] Minor fix. --- src/modules/flow_led/flow_led.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c index d90e913dbfcd..2b2f8df830a3 100644 --- a/src/modules/flow_led/flow_led.c +++ b/src/modules/flow_led/flow_led.c @@ -208,11 +208,12 @@ int flow_led_thread_main(int argc, char *argv[]) { /* Update LED output PWM value */ /* TEMP: Blink LED */ - if (led_out < 0.5f && !(loop_counter % 10)){ + if (led_out < 0.5f && (loop_counter % 10 == 0)){ led_out = 1.0f; mavlink_log_info(mavlink_fd, "Led output: %.2f", led_out) - } else if (led_out < 0.5f && !(loop_counter % 10)) { + } else if (led_out > 0.5f && (loop_counter % 10 == 0)) { led_out = 0.0f; + mavlink_log_info(mavlink_fd, "Led output: %.2f", led_out) } /* Limit LED output */ From 9afaf46ffa4beed6d96bc220192c6c22d75fe007 Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 15 Jul 2014 16:29:49 -0400 Subject: [PATCH 13/17] Change poll wait time and dt constraint. --- src/modules/flow_led/flow_led.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c index 2b2f8df830a3..2245f35b22ff 100644 --- a/src/modules/flow_led/flow_led.c +++ b/src/modules/flow_led/flow_led.c @@ -171,13 +171,13 @@ int flow_led_thread_main(int argc, char *argv[]) { while (!thread_should_exit) { - /* Wait for update for 500 ms */ - int poll_result = poll(fds, 1, 500); + /* Wait for update for 1000 ms */ + int poll_result = poll(fds, 1, 1000); hrt_abstime t = hrt_absolute_time(); /* Calculate time difference since last iteration of loop */ float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; - dt = fmaxf(fminf(0.5, dt), 0.05); // Constrain dt from 2 to 500 ms + dt = fmaxf(fminf(0.05, dt), 0.005); // Constrain dt from 5 to 50 ms t_prev = t; if (poll_result == 0) { @@ -208,10 +208,10 @@ int flow_led_thread_main(int argc, char *argv[]) { /* Update LED output PWM value */ /* TEMP: Blink LED */ - if (led_out < 0.5f && (loop_counter % 10 == 0)){ + if (led_out < 0.5f && (loop_counter % 2 == 0)){ led_out = 1.0f; mavlink_log_info(mavlink_fd, "Led output: %.2f", led_out) - } else if (led_out > 0.5f && (loop_counter % 10 == 0)) { + } else if (led_out > 0.5f && (loop_counter % 2 == 0)) { led_out = 0.0f; mavlink_log_info(mavlink_fd, "Led output: %.2f", led_out) } From f20c531d424d4178da92fb45f7dde9d1d4a3ef5a Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Tue, 15 Jul 2014 22:48:00 -0400 Subject: [PATCH 14/17] Added support for LED PID controller. --- src/modules/flow_led/flow_led.c | 40 ++++++++++++++++++++++++--------- 1 file changed, 30 insertions(+), 10 deletions(-) diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c index 2245f35b22ff..4438abe9aaf9 100644 --- a/src/modules/flow_led/flow_led.c +++ b/src/modules/flow_led/flow_led.c @@ -57,6 +57,9 @@ #include #include #include +// TODO: Include parameters for: +// PID controller +// Flow Q setpoint __EXPORT int flow_led_main(int argc, char *argv[]); @@ -163,10 +166,13 @@ int flow_led_thread_main(int argc, char *argv[]) { /* Variable initializations */ int error_counter = 0; int loop_counter = 0; - int flow_q = 0; - float sonar = 0.0f; - float sonar_avg = 0.0f; - float led_out = 0.0f; + int flow_q = 0; // Flow quality factor + int flow_q_err = 0; // Flow quality error from setpoint parameter + float sonar = 0.0f; // Sonar height measurement + float sonar_avg = 0.0f; // Sonar average height measurement + float led_out = 0.0f; // LED output intensity (0.0 - 1.0) + float led_int = 0.0f; // LED output integral + float led_err = 0.0f hrt_abstime t_prev = 0; while (!thread_should_exit) { @@ -182,13 +188,16 @@ int flow_led_thread_main(int argc, char *argv[]) { if (poll_result == 0) { /* No new flow data */ - // printf("[flow_led] Got no data within 500ms \n"); - + if (verbose_mode) { + printf("[flow_led] Got no data within a second. \n"); + } } else if (poll_result < 0) { /* ERROR */ if (error_counter < 10 || error_counter % 50 == 0) { /* Use error counter to prevent flooding */ - // printf("[flow_led] ERROR return value from poll(): %d\n", poll_result); + if (verbose_mode) { + printf("[flow_led] ERROR return value from poll(): %d\n", poll_result); + } } error_counter++; @@ -202,12 +211,15 @@ int flow_led_thread_main(int argc, char *argv[]) { flow_q = flow.quality; sonar = flow.ground_distance_m; - // TODO: Update led_out with respect to flow quality and sonar height + // TODO: Update flow quality error using setpoint parameter + flow_q_err = 240 - flow_q ; + led_err = flow_q_err / 255.0f; + } } - /* Update LED output PWM value */ - /* TEMP: Blink LED */ + /* + // TEMP: Blink LED if (led_out < 0.5f && (loop_counter % 2 == 0)){ led_out = 1.0f; mavlink_log_info(mavlink_fd, "Led output: %.2f", led_out) @@ -215,6 +227,14 @@ int flow_led_thread_main(int argc, char *argv[]) { led_out = 0.0f; mavlink_log_info(mavlink_fd, "Led output: %.2f", led_out) } + */ + + // TODO: Sonar average calculation + + /* LED PID control */ + // led_int += led_err * led_i * dt; + // led_out += led_err * led_p + (led_err_prev - led_err) * led_d / dt + led_int; + // led_err_prev = led_err; /* Limit LED output */ if (led_out > 1.0f){ From e3a2576709a9f92d31f0f37dcbe742076c7def80 Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Wed, 16 Jul 2014 04:56:49 -0400 Subject: [PATCH 15/17] Added parameter support and subscribed to manual control setpoint. --- src/modules/flow_led/flow_led.c | 71 ++++++++++++++++++++------ src/modules/flow_led/flow_led_params.c | 66 ++++++++++++++++++++++++ src/modules/flow_led/flow_led_params.h | 67 ++++++++++++++++++++++++ src/modules/flow_led/module.mk | 3 +- 4 files changed, 190 insertions(+), 17 deletions(-) create mode 100644 src/modules/flow_led/flow_led_params.c create mode 100644 src/modules/flow_led/flow_led_params.h diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c index 4438abe9aaf9..27d6ce1ccbda 100644 --- a/src/modules/flow_led/flow_led.c +++ b/src/modules/flow_led/flow_led.c @@ -53,13 +53,13 @@ #include #include #include +#include #include #include #include #include -// TODO: Include parameters for: -// PID controller -// Flow Q setpoint + +#include "flow_led_params.h" __EXPORT int flow_led_main(int argc, char *argv[]); @@ -144,23 +144,41 @@ int flow_led_thread_main(int argc, char *argv[]) { /* Initialize structs */ struct optical_flow_s flow; memset(&flow, 0, sizeof(flow)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); - /* Subscribe to flow topic */ + /* Subscribe to uORB topics */ int flow_sub = orb_subscribe(ORB_ID(optical_flow)); + int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); /* Advertise on actuators topic */ orb_advert_t actuators_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators); + /* Initialize parameter handles */ + struct flow_led_params params; + struct flow_led_param_handles flow_led_param_handles; + parameters_init(&flow_led_param_handles); + + /* First parameter read */ + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); + + /* First parameter update */ + parameters_update(&flow_led_param_handles, ¶ms); + /* Initialize MAVLink fd for output to QGC */ int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - // mavlink_log_info(mavlink_fd, "[inav] started"); + mavlink_log_info(mavlink_fd, "[LED] started"); /* Polling fds */ struct pollfd fds[] = { - { .fd = flow_sub, .events = POLLIN } + { .fd = flow_sub, .events = POLLIN }, + // Poll manual sp for debugging purposes + { .fd = manual_sub, .events = POLLIN } }; /* Variable initializations */ @@ -172,13 +190,14 @@ int flow_led_thread_main(int argc, char *argv[]) { float sonar_avg = 0.0f; // Sonar average height measurement float led_out = 0.0f; // LED output intensity (0.0 - 1.0) float led_int = 0.0f; // LED output integral - float led_err = 0.0f - hrt_abstime t_prev = 0; + float led_err = 0.0f; // LED error calculated from flow Q error + float throttle = 0.0f; // Current throttle stick position + hrt_abstime t_prev = 0; // Absolute time of previous iteration of main loop while (!thread_should_exit) { /* Wait for update for 1000 ms */ - int poll_result = poll(fds, 1, 1000); + int poll_result = poll(fds, 2, 1000); hrt_abstime t = hrt_absolute_time(); /* Calculate time difference since last iteration of loop */ @@ -202,9 +221,18 @@ int flow_led_thread_main(int argc, char *argv[]) { error_counter++; } else { + + /* Parameter update */ + bool updated; + orb_check(parameter_update_sub, &updated); + if (updated) { + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); + parameters_update(&flow_led_param_handles, ¶ms); + } + /* Poll flow data */ if (fds[0].revents & POLLIN) { - /* Flow data obtained */ /* Copy flow data to local buffer */ orb_copy(ORB_ID(optical_flow), flow_sub, &flow); @@ -215,6 +243,21 @@ int flow_led_thread_main(int argc, char *argv[]) { flow_q_err = 240 - flow_q ; led_err = flow_q_err / 255.0f; + // TODO: Sonar average calculation + + /* LED PID control */ + // led_int += led_err * params.led_i * dt; + // led_out += led_err * params.led_p + (led_err_prev - led_err) * params.led_d / dt + led_int; + // led_err_prev = led_err; + + } + + /* Poll manual setpoint for debugging purposes */ + if (fds[1].revents & POLLIN) { + + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + throttle = manual.z; + } } @@ -229,12 +272,8 @@ int flow_led_thread_main(int argc, char *argv[]) { } */ - // TODO: Sonar average calculation - - /* LED PID control */ - // led_int += led_err * led_i * dt; - // led_out += led_err * led_p + (led_err_prev - led_err) * led_d / dt + led_int; - // led_err_prev = led_err; + // TEMP: Assign throttle stick to LED output + led_out = throttle; /* Limit LED output */ if (led_out > 1.0f){ diff --git a/src/modules/flow_led/flow_led_params.c b/src/modules/flow_led/flow_led_params.c new file mode 100644 index 000000000000..4d178dabb1c9 --- /dev/null +++ b/src/modules/flow_led/flow_led_params.c @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (C) 2014 Antonio Sanniravong. All rights reserved. + * Author: Antonio Sanniravong + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file flow_led_params.c + * + * Parameters for PX4Flow assistive LED controller + */ + +#include "flow_led_params.h" + +PARAM_DEFINE_FLOAT(LED_P, 0.5f); +PARAM_DEFINE_FLOAT(LED_I, 0.0f); +PARAM_DEFINE_FLOAT(LED_D, 0.0f); +PARAM_DEFINE_INT32(LED_FLOW_Q_SP, 240); + +int parameters_init(struct flow_led_param_handles *h) +{ + h->led_p = param_find("LED_P"); + h->led_i = param_find("LED_I"); + h->led_d = param_find("LED_D"); + h->flow_q_sp = param_find("LED_FLOW_Q_SP"); + + return OK; +} + +int parameters_update(const struct flow_led_param_handles *h, struct flow_led_params *p) +{ + param_get(h->led_p, &(p->led_p)); + param_get(h->led_i, &(p->led_i)); + param_get(h->led_d, &(p->led_d)); + param_get(h->flow_q_sp, &(p->flow_q_sp)); + + return OK; +} diff --git a/src/modules/flow_led/flow_led_params.h b/src/modules/flow_led/flow_led_params.h new file mode 100644 index 000000000000..cfd6fde02a4c --- /dev/null +++ b/src/modules/flow_led/flow_led_params.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (C) 2014 Antonio Sanniravong. All rights reserved. + * Author: Antonio Sanniravong + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file flow_led_params.h + * + * Parameters for PX4Flow assistive LED controller + */ + +#include + +struct flow_led_params { + float led_p; + float led_i; + float led_d; + int flow_q_sp; +}; + +struct flow_led_param_handles { + param_t led_p; + param_t led_i; + param_t led_d; + param_t flow_q_sp; +}; + +/** + * Initialize all parameter handles and values + * + */ +int parameters_init(struct flow_led_param_handles *h); + +/** + * Update all parameters + * + */ +int parameters_update(const struct flow_led_param_handles *h, struct flow_led_params *p); diff --git a/src/modules/flow_led/module.mk b/src/modules/flow_led/module.mk index 0ffde2b18f4c..d8f40ca8eb01 100644 --- a/src/modules/flow_led/module.mk +++ b/src/modules/flow_led/module.mk @@ -1,2 +1,3 @@ MODULE_COMMAND = flow_led -SRCS = flow_led.c \ No newline at end of file +SRCS = flow_led.c \ + flow_led_params.c \ No newline at end of file From 697dc30728b9964bdbb054c201861ebf7b8dfa3b Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Wed, 16 Jul 2014 05:07:32 -0400 Subject: [PATCH 16/17] Added missing uORB parameter topic. --- src/modules/flow_led/flow_led.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c index 27d6ce1ccbda..1c5c3d2e60ad 100644 --- a/src/modules/flow_led/flow_led.c +++ b/src/modules/flow_led/flow_led.c @@ -54,6 +54,7 @@ #include #include #include + #include #include #include #include From 58928c962bc2da255c8ea266135259e54e3589b9 Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Wed, 16 Jul 2014 18:25:17 -0400 Subject: [PATCH 17/17] Allow shorter PWM pulses & activate LED PID control. --- src/drivers/drv_pwm_output.h | 2 +- src/modules/flow_led/flow_led.c | 17 +++++++++++------ 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 6fdb09e3e0ea..25dd18f6aa78 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -67,7 +67,7 @@ __BEGIN_DECLS /** * Lowest minimum PWM in us */ -#define PWM_LOWEST_MIN 100 +#define PWM_LOWEST_MIN 1 /** * Default minimum PWM in us diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c index 1c5c3d2e60ad..3bcdcefde03c 100644 --- a/src/modules/flow_led/flow_led.c +++ b/src/modules/flow_led/flow_led.c @@ -192,6 +192,7 @@ int flow_led_thread_main(int argc, char *argv[]) { float led_out = 0.0f; // LED output intensity (0.0 - 1.0) float led_int = 0.0f; // LED output integral float led_err = 0.0f; // LED error calculated from flow Q error + float led_err_prev = 0.0f; // LED previous error float throttle = 0.0f; // Current throttle stick position hrt_abstime t_prev = 0; // Absolute time of previous iteration of main loop @@ -240,26 +241,28 @@ int flow_led_thread_main(int argc, char *argv[]) { flow_q = flow.quality; sonar = flow.ground_distance_m; - // TODO: Update flow quality error using setpoint parameter - flow_q_err = 240 - flow_q ; + /* Update flow quality error using setpoint parameter */ + flow_q_err = params.flow_q_sp - flow_q ; led_err = flow_q_err / 255.0f; // TODO: Sonar average calculation /* LED PID control */ - // led_int += led_err * params.led_i * dt; - // led_out += led_err * params.led_p + (led_err_prev - led_err) * params.led_d / dt + led_int; - // led_err_prev = led_err; + led_int += led_err * params.led_i * dt; + led_out += led_err * params.led_p + (led_err_prev - led_err) * params.led_d / dt + led_int; + led_err_prev = led_err; } - /* Poll manual setpoint for debugging purposes */ + /* + // Poll manual setpoint for debugging purposes if (fds[1].revents & POLLIN) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); throttle = manual.z; } + */ } /* @@ -273,8 +276,10 @@ int flow_led_thread_main(int argc, char *argv[]) { } */ + /* // TEMP: Assign throttle stick to LED output led_out = throttle; + */ /* Limit LED output */ if (led_out > 1.0f){