From 09e60893df954fe69a32a7ebfc54fbb715f61197 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 9 May 2013 09:37:42 +0400 Subject: gpio_led app added --- apps/gpio_led/Makefile | 42 +++++++++ apps/gpio_led/gpio_led_main.c | 184 +++++++++++++++++++++++++++++++++++++ nuttx/configs/px4fmu/nsh/appconfig | 1 + 3 files changed, 227 insertions(+) create mode 100644 apps/gpio_led/Makefile create mode 100644 apps/gpio_led/gpio_led_main.c diff --git a/apps/gpio_led/Makefile b/apps/gpio_led/Makefile new file mode 100644 index 000000000..e9dc219ea --- /dev/null +++ b/apps/gpio_led/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# +# 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. +# +############################################################################ + +# +# Makefile to build gpio_led +# + +APPNAME = gpio_led +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/gpio_led/gpio_led_main.c b/apps/gpio_led/gpio_led_main.c new file mode 100644 index 000000000..c0ad84da1 --- /dev/null +++ b/apps/gpio_led/gpio_led_main.c @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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 gpio_led.c + * + * Drive status LED via GPIO. + * + * @author Anton Babushkin + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static bool thread_should_exit = false; +static bool thread_running = false; + +__EXPORT int gpio_led_main(int argc, char *argv[]); + +static int gpio_led_thread_main(int argc, char *argv[]); + +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: gpio_led {start|stop|status}\n\n"); + exit(1); +} + +int gpio_led_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("gpio_led already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + task_spawn("gpio_led", + SCHED_DEFAULT, + SCHED_PRIORITY_MIN, + 2048, + gpio_led_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tgpio_led is running\n"); + } else { + printf("\tgpio_led not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int gpio_led_thread_main(int argc, char *argv[]) +{ + /* welcome user */ + printf("[gpio_led] started\n"); + + int fd = open(GPIO_DEVICE_PATH, 0); + if (fd < 0) { + printf("[gpio_led] GPIO: open fail\n"); + return ERROR; + } + + /* set GPIO EXT 1 as output */ + ioctl(fd, GPIO_SET_OUTPUT, GPIO_EXT_1); + + ioctl(fd, GPIO_CLEAR, GPIO_EXT_1); + + /* initialize values */ + bool led_state = false; + int counter = 0; + + /* subscribe to vehicle status topic */ + struct vehicle_status_s status; + memset(&status, 0, sizeof(status)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + while (!thread_should_exit) { + bool status_updated; + orb_check((vehicle_status_sub), &status_updated); + if (status_updated) + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); + + int pattern = 0; + if (status.flag_system_armed) { + if (status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + pattern = 0x3f; // ****** solid (armed) + } else { + pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning) + } + } else { + if (status.state_machine == SYSTEM_STATE_PREFLIGHT) { + pattern = 0x00; // ______ off (disarmed, preflight check) + } else if (status.state_machine == SYSTEM_STATE_STANDBY && status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + pattern = 0x38; // ***___ slow blink (disarmed, ready) + } else { + pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm) + } + } + + bool led_state_new = (pattern & (1 << counter)) != 0; + if (led_state_new != led_state) { + led_state = led_state_new; + if (led_state) { + ioctl(fd, GPIO_SET, GPIO_EXT_1); + } else { + ioctl(fd, GPIO_CLEAR, GPIO_EXT_1); + } + } + + counter++; + if (counter > 5) + counter = 0; + + usleep(333333); // sleep ~1/3s + } + + ioctl(fd, GPIO_CLEAR, GPIO_EXT_1); + + printf("[gpio_led] exiting\n"); + + return 0; +} diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 82ab94be7..e0ad9fb1a 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -103,6 +103,7 @@ CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator CONFIGURED_APPS += attitude_estimator_ekf CONFIGURED_APPS += hott_telemetry +CONFIGURED_APPS += gpio_led endif # Hacking tools -- cgit v1.2.3 From 0489595e7f34f4bacc1b63f31eed5e64fe55c7e6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 9 May 2013 19:44:09 +0400 Subject: Use work_queue instead of thread --- apps/gpio_led/gpio_led_main.c | 186 ++++++++++++++++++------------------------ 1 file changed, 81 insertions(+), 105 deletions(-) diff --git a/apps/gpio_led/gpio_led_main.c b/apps/gpio_led/gpio_led_main.c index c0ad84da1..64ede01c3 100644 --- a/apps/gpio_led/gpio_led_main.c +++ b/apps/gpio_led/gpio_led_main.c @@ -45,140 +45,116 @@ #include #include #include -#include +#include +#include #include #include #include #include #include -static bool thread_should_exit = false; -static bool thread_running = false; -__EXPORT int gpio_led_main(int argc, char *argv[]); - -static int gpio_led_thread_main(int argc, char *argv[]); - -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: gpio_led {start|stop|status}\n\n"); - exit(1); -} - -int gpio_led_main(int argc, char *argv[]) +struct gpio_led_s { - if (argc < 1) - usage("missing command"); + struct work_s work; + int gpio_fd; + struct vehicle_status_s status; + int vehicle_status_sub; + bool led_state; + int counter; +}; - if (!strcmp(argv[1], "start")) { +static struct gpio_led_s gpio_led; - if (thread_running) { - printf("gpio_led already running\n"); - /* this is not an error */ - exit(0); - } +__EXPORT int gpio_led_main(int argc, char *argv[]); - thread_should_exit = false; - task_spawn("gpio_led", - SCHED_DEFAULT, - SCHED_PRIORITY_MIN, - 2048, - gpio_led_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } +void gpio_led_start(FAR void *arg); - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } +void gpio_led_cycle(FAR void *arg); - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tgpio_led is running\n"); - } else { - printf("\tgpio_led not started\n"); - } - exit(0); +int gpio_led_main(int argc, char *argv[]) +{ + memset(&gpio_led, 0, sizeof(gpio_led)); + int ret = work_queue(LPWORK, &gpio_led.work, gpio_led_start, &gpio_led, 0); + if (ret != 0) { + printf("[gpio_led] Failed to queue work: %d\n", ret); + exit(1); } - - usage("unrecognized command"); - exit(1); + exit(0); } -int gpio_led_thread_main(int argc, char *argv[]) +void gpio_led_start(FAR void *arg) { - /* welcome user */ - printf("[gpio_led] started\n"); + FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg; - int fd = open(GPIO_DEVICE_PATH, 0); - if (fd < 0) { + /* open GPIO device */ + priv->gpio_fd = open(GPIO_DEVICE_PATH, 0); + if (priv->gpio_fd < 0) { printf("[gpio_led] GPIO: open fail\n"); - return ERROR; + return; } - /* set GPIO EXT 1 as output */ - ioctl(fd, GPIO_SET_OUTPUT, GPIO_EXT_1); + /* configure GPIO pin */ + ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, GPIO_EXT_1); - ioctl(fd, GPIO_CLEAR, GPIO_EXT_1); + /* subscribe to vehicle status topic */ + memset(&priv->status, 0, sizeof(priv->status)); + priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + + /* add worker to queue */ + int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0); + if (ret != 0) { + printf("[gpio_led] Failed to queue work: %d\n", ret); + return; + } - /* initialize values */ - bool led_state = false; - int counter = 0; + printf("[gpio_led] Started\n"); +} - /* subscribe to vehicle status topic */ - struct vehicle_status_s status; - memset(&status, 0, sizeof(status)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - - while (!thread_should_exit) { - bool status_updated; - orb_check((vehicle_status_sub), &status_updated); - if (status_updated) - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &status); - - int pattern = 0; - if (status.flag_system_armed) { - if (status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { - pattern = 0x3f; // ****** solid (armed) - } else { - pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning) - } +void gpio_led_cycle(FAR void *arg) +{ + FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg; + + /* check for status updates*/ + bool status_updated; + orb_check(priv->vehicle_status_sub, &status_updated); + if (status_updated) + orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); + + /* select pattern for current status */ + int pattern = 0; + if (priv->status.flag_system_armed) { + if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + pattern = 0x3f; // ****** solid (armed) } else { - if (status.state_machine == SYSTEM_STATE_PREFLIGHT) { - pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (status.state_machine == SYSTEM_STATE_STANDBY && status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { - pattern = 0x38; // ***___ slow blink (disarmed, ready) - } else { - pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm) - } + pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning) } - - bool led_state_new = (pattern & (1 << counter)) != 0; - if (led_state_new != led_state) { - led_state = led_state_new; - if (led_state) { - ioctl(fd, GPIO_SET, GPIO_EXT_1); - } else { - ioctl(fd, GPIO_CLEAR, GPIO_EXT_1); - } + } else { + if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) { + pattern = 0x00; // ______ off (disarmed, preflight check) + } else if (priv->status.state_machine == SYSTEM_STATE_STANDBY && + priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + pattern = 0x38; // ***___ slow blink (disarmed, ready) + } else { + pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm) } - - counter++; - if (counter > 5) - counter = 0; - - usleep(333333); // sleep ~1/3s } - ioctl(fd, GPIO_CLEAR, GPIO_EXT_1); + /* blink pattern */ + bool led_state_new = (pattern & (1 << priv->counter)) != 0; + if (led_state_new != priv->led_state) { + priv->led_state = led_state_new; + if (led_state_new) { + ioctl(priv->gpio_fd, GPIO_SET, GPIO_EXT_1); + } else { + ioctl(priv->gpio_fd, GPIO_CLEAR, GPIO_EXT_1); + } + } - printf("[gpio_led] exiting\n"); + priv->counter++; + if (priv->counter > 5) + priv->counter = 0; - return 0; + /* repeat cycle at 5 Hz*/ + work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000)); } -- cgit v1.2.3 From 09ce3e2d0a531138c29e1d32f1a9a902b259683d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 May 2013 11:30:50 +0400 Subject: Added GPIO_EXT1/GPIO_EXT2 selection. --- src/modules/gpio_led/gpio_led.c | 30 +++++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index b9b066d24..83b598e92 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -57,13 +57,14 @@ struct gpio_led_s { struct work_s work; int gpio_fd; + int pin; struct vehicle_status_s status; int vehicle_status_sub; bool led_state; int counter; }; -static struct gpio_led_s gpio_led; +static struct gpio_led_s gpio_led_data; __EXPORT int gpio_led_main(int argc, char *argv[]); @@ -73,8 +74,23 @@ void gpio_led_cycle(FAR void *arg); int gpio_led_main(int argc, char *argv[]) { - memset(&gpio_led, 0, sizeof(gpio_led)); - int ret = work_queue(LPWORK, &gpio_led.work, gpio_led_start, &gpio_led, 0); + int pin = GPIO_EXT_1; + if (argc > 1) { + if (!strcmp(argv[1], "-p")) { + if (!strcmp(argv[2], "1")) { + pin = GPIO_EXT_1; + } else if (!strcmp(argv[2], "2")) { + pin = GPIO_EXT_2; + } else { + printf("[gpio_led] Unsupported pin: %s\n", argv[2]); + exit(1); + } + } + } + + memset(&gpio_led_data, 0, sizeof(gpio_led_data)); + gpio_led_data.pin = pin; + int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0); if (ret != 0) { printf("[gpio_led] Failed to queue work: %d\n", ret); exit(1); @@ -94,7 +110,7 @@ void gpio_led_start(FAR void *arg) } /* configure GPIO pin */ - ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, GPIO_EXT_1); + ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin); /* subscribe to vehicle status topic */ memset(&priv->status, 0, sizeof(priv->status)); @@ -107,7 +123,7 @@ void gpio_led_start(FAR void *arg) return; } - printf("[gpio_led] Started\n"); + printf("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin); } void gpio_led_cycle(FAR void *arg) @@ -144,9 +160,9 @@ void gpio_led_cycle(FAR void *arg) if (led_state_new != priv->led_state) { priv->led_state = led_state_new; if (led_state_new) { - ioctl(priv->gpio_fd, GPIO_SET, GPIO_EXT_1); + ioctl(priv->gpio_fd, GPIO_SET, priv->pin); } else { - ioctl(priv->gpio_fd, GPIO_CLEAR, GPIO_EXT_1); + ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); } } -- cgit v1.2.3 From 6e8621269bc032afdb77830cebac01808299263d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 May 2013 13:59:51 +0400 Subject: Code style fixed --- src/modules/gpio_led/gpio_led.c | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 83b598e92..a80bf9cb8 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -53,8 +53,7 @@ #include #include -struct gpio_led_s -{ +struct gpio_led_s { struct work_s work; int gpio_fd; int pin; @@ -75,12 +74,15 @@ void gpio_led_cycle(FAR void *arg); int gpio_led_main(int argc, char *argv[]) { int pin = GPIO_EXT_1; + if (argc > 1) { if (!strcmp(argv[1], "-p")) { if (!strcmp(argv[2], "1")) { pin = GPIO_EXT_1; + } else if (!strcmp(argv[2], "2")) { pin = GPIO_EXT_2; + } else { printf("[gpio_led] Unsupported pin: %s\n", argv[2]); exit(1); @@ -91,10 +93,12 @@ int gpio_led_main(int argc, char *argv[]) memset(&gpio_led_data, 0, sizeof(gpio_led_data)); gpio_led_data.pin = pin; int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0); + if (ret != 0) { printf("[gpio_led] Failed to queue work: %d\n", ret); exit(1); } + exit(0); } @@ -104,6 +108,7 @@ void gpio_led_start(FAR void *arg) /* open GPIO device */ priv->gpio_fd = open(GPIO_DEVICE_PATH, 0); + if (priv->gpio_fd < 0) { printf("[gpio_led] GPIO: open fail\n"); return; @@ -118,6 +123,7 @@ void gpio_led_start(FAR void *arg) /* add worker to queue */ int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0); + if (ret != 0) { printf("[gpio_led] Failed to queue work: %d\n", ret); return; @@ -133,23 +139,29 @@ void gpio_led_cycle(FAR void *arg) /* check for status updates*/ bool status_updated; orb_check(priv->vehicle_status_sub, &status_updated); + if (status_updated) orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); /* select pattern for current status */ int pattern = 0; + if (priv->status.flag_system_armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) + } else { pattern = 0x2A; // *_*_*_ fast blink (armed, battery warning) } + } else { if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) { pattern = 0x00; // ______ off (disarmed, preflight check) + } else if (priv->status.state_machine == SYSTEM_STATE_STANDBY && - priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) + } else { pattern = 0x28; // *_*___ slow double blink (disarmed, not good to arm) } @@ -157,16 +169,20 @@ void gpio_led_cycle(FAR void *arg) /* blink pattern */ bool led_state_new = (pattern & (1 << priv->counter)) != 0; + if (led_state_new != priv->led_state) { priv->led_state = led_state_new; + if (led_state_new) { ioctl(priv->gpio_fd, GPIO_SET, priv->pin); + } else { ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); } } priv->counter++; + if (priv->counter > 5) priv->counter = 0; -- cgit v1.2.3