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) 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/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 972573f9ff2b..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 900 +#define PWM_LOWEST_MIN 1 /** * 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 diff --git a/src/modules/flow_led/flow_led.c b/src/modules/flow_led/flow_led.c new file mode 100644 index 000000000000..3bcdcefde03c --- /dev/null +++ b/src/modules/flow_led/flow_led.c @@ -0,0 +1,304 @@ +/**************************************************************************** + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include +#include +#include +#include +#include + +#include "flow_led_params.h" + +__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) { + 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)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + + /* 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, "[LED] started"); + + /* Polling fds */ + struct pollfd fds[] = { + { .fd = flow_sub, .events = POLLIN }, + // Poll manual sp for debugging purposes + { .fd = manual_sub, .events = POLLIN } + }; + + /* Variable initializations */ + int error_counter = 0; + int loop_counter = 0; + 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; // 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 + + while (!thread_should_exit) { + + /* Wait for update for 1000 ms */ + int poll_result = poll(fds, 2, 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.05, dt), 0.005); // Constrain dt from 5 to 50 ms + t_prev = t; + + if (poll_result == 0) { + /* No new flow data */ + 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 */ + if (verbose_mode) { + printf("[flow_led] ERROR return value from poll(): %d\n", poll_result); + } + } + 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) { + + /* Copy flow data to local buffer */ + orb_copy(ORB_ID(optical_flow), flow_sub, &flow); + flow_q = flow.quality; + sonar = flow.ground_distance_m; + + /* 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; + + } + + /* + // Poll manual setpoint for debugging purposes + if (fds[1].revents & POLLIN) { + + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + throttle = manual.z; + + } + */ + } + + /* + // 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) + } else if (led_out > 0.5f && (loop_counter % 2 == 0)) { + led_out = 0.0f; + mavlink_log_info(mavlink_fd, "Led output: %.2f", led_out) + } + */ + + /* + // TEMP: Assign throttle stick to LED output + led_out = throttle; + */ + + /* 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[0] = led_out; + actuators.timestamp = t; + + /* Publish PWM output */ + orb_publish(ORB_ID(actuator_controls_1), actuators_pub, &actuators); + + loop_counter++; + + } + warnx("stopped"); + thread_running = false; + return 0; +} 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 new file mode 100644 index 000000000000..d8f40ca8eb01 --- /dev/null +++ b/src/modules/flow_led/module.mk @@ -0,0 +1,3 @@ +MODULE_COMMAND = flow_led +SRCS = flow_led.c \ + flow_led_params.c \ No newline at end of file