diff options
author | px4dev <px4@purgatory.org> | 2012-10-31 00:37:15 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-10-31 00:37:15 -0700 |
commit | 34a3b260f34398994a315388b3ffed22a1fe22fb (patch) | |
tree | cbea6c3fff4e2701921827b122df9d348c38ac1c /apps | |
parent | b685d46dbfc583a26a92f1466dca64f9d45c3c4e (diff) | |
download | px4-firmware-34a3b260f34398994a315388b3ffed22a1fe22fb.tar.gz px4-firmware-34a3b260f34398994a315388b3ffed22a1fe22fb.tar.bz2 px4-firmware-34a3b260f34398994a315388b3ffed22a1fe22fb.zip |
Move the last of the board-specific code for PX4FMU out of the NuttX tree. Now it's just configuration.
Diffstat (limited to 'apps')
-rw-r--r-- | apps/commander/commander.c | 4 | ||||
-rw-r--r-- | apps/drivers/boards/px4fmu/px4fmu_init.c | 64 | ||||
-rw-r--r-- | apps/drivers/boards/px4fmu/px4fmu_led.c | 88 | ||||
-rw-r--r-- | apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c | 10 | ||||
-rw-r--r-- | apps/drivers/drv_led.h | 64 | ||||
-rw-r--r-- | apps/drivers/led/Makefile (renamed from apps/systemcmds/led/Makefile) | 6 | ||||
-rw-r--r-- | apps/drivers/led/led.cpp | 115 | ||||
-rw-r--r-- | apps/px4/tests/test_float.c | 2 | ||||
-rw-r--r-- | apps/px4/tests/test_int.c | 2 | ||||
-rw-r--r-- | apps/px4/tests/test_led.c | 4 | ||||
-rw-r--r-- | apps/px4/tests/test_sleep.c | 2 | ||||
-rw-r--r-- | apps/px4/tests/test_uart_baudchange.c | 2 | ||||
-rw-r--r-- | apps/px4/tests/test_uart_console.c | 2 | ||||
-rw-r--r-- | apps/px4/tests/test_uart_loopback.c | 2 | ||||
-rw-r--r-- | apps/px4/tests/test_uart_send.c | 2 | ||||
-rw-r--r-- | apps/px4/tests/tests.h | 1 | ||||
-rw-r--r-- | apps/systemcmds/led/led.c | 209 |
17 files changed, 278 insertions, 301 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index e00dc13d2..bf9d07c9b 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -61,7 +61,7 @@ #include <sys/prctl.h> #include <v1.0/common/mavlink.h> #include <string.h> -#include <arch/board/drv_led.h> +#include <drivers/drv_led.h> #include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h> #include <drivers/drv_tone_alarm.h> @@ -194,7 +194,7 @@ static void buzzer_deinit() static int led_init() { - leds = open("/dev/led", O_RDONLY | O_NONBLOCK); + leds = open(LED_DEVICE_PATH, 0); if (leds < 0) { fprintf(stderr, "[commander] LED: open fail\n"); diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index 38a284017..568d861c9 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -64,9 +64,9 @@ #include "stm32_uart.h" #include <arch/board/board.h> -#include <arch/board/drv_led.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_led.h> #include <systemlib/cpuload.h> @@ -131,9 +131,6 @@ __EXPORT void stm32_boardinitialize(void) static struct spi_dev_s *spi1; static struct spi_dev_s *spi3; -static struct i2c_dev_s *i2c1; -static struct i2c_dev_s *i2c2; -static struct i2c_dev_s *i2c3; #include <math.h> @@ -153,10 +150,6 @@ __EXPORT int nsh_archinitialize(void) { int result; - /* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */ - - /* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */ - /* configure the high-resolution time/callout interface */ #ifdef CONFIG_HRT_TIMER hrt_init(); @@ -190,14 +183,12 @@ __EXPORT int nsh_archinitialize(void) message("\r\n"); + // initial LED state + drv_led_start(); up_ledoff(LED_BLUE); up_ledoff(LED_AMBER); - up_ledon(LED_BLUE); - /* Configure user-space led driver */ - px4fmu_led_init(); - /* Configure SPI-based devices */ spi1 = up_spiinitialize(1); @@ -219,38 +210,6 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Successfully initialized SPI port 1\r\n"); -#if 0 - /* initialize I2C2 bus */ - - i2c2 = up_i2cinitialize(2); - - if (!i2c2) { - message("[boot] FAILED to initialize I2C bus 2\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* set I2C2 speed */ - I2C_SETFREQUENCY(i2c2, 400000); - - - i2c3 = up_i2cinitialize(3); - - if (!i2c3) { - message("[boot] FAILED to initialize I2C bus 3\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* set I2C3 speed */ - I2C_SETFREQUENCY(i2c3, 400000); - - /* try to attach, don't fail if device is not responding */ - (void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS, - FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES, - FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES, - FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1); -#endif #if defined(CONFIG_STM32_SPI3) /* Get the SPI port */ @@ -276,24 +235,7 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n"); #endif /* SPI3 */ -#if 0 - /* initialize I2C1 bus */ - - i2c1 = up_i2cinitialize(1); - if (!i2c1) { - message("[boot] FAILED to initialize I2C bus 1\n"); - up_ledon(LED_AMBER); - return -ENODEV; - } - - /* set I2C1 speed */ - I2C_SETFREQUENCY(i2c1, 400000); - - /* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */ - - /* Get board information if available */ -#endif #ifdef CONFIG_ADC int adc_state = adc_devinit(); diff --git a/apps/drivers/boards/px4fmu/px4fmu_led.c b/apps/drivers/boards/px4fmu/px4fmu_led.c new file mode 100644 index 000000000..fd1e159aa --- /dev/null +++ b/apps/drivers/boards/px4fmu/px4fmu_led.c @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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. + * + ****************************************************************************/ + +/** + * @file px4fmu_led.c + * + * PX4FMU LED backend. + */ + +#include <nuttx/config.h> + +#include <stdint.h> +#include <stdbool.h> +#include <debug.h> + +#include <arch/board/board.h> + +#include "chip.h" +#include "up_arch.h" +#include "up_internal.h" +#include "stm32_internal.h" +#include "px4fmu_internal.h" + +__EXPORT void up_ledinit() +{ + /* Configure LED1-2 GPIOs for output */ + + stm32_configgpio(GPIO_LED1); + stm32_configgpio(GPIO_LED2); +} + +__EXPORT void up_ledon(int led) +{ + if (led == 0) + { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED1, false); + } + if (led == 1) + { + /* Pull down to switch on */ + stm32_gpiowrite(GPIO_LED2, false); + } +} + +__EXPORT void up_ledoff(int led) +{ + if (led == 0) + { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED1, true); + } + if (led == 1) + { + /* Pull up to switch off */ + stm32_gpiowrite(GPIO_LED2, true); + } +} diff --git a/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c index d1ff8c112..cb8918306 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c +++ b/apps/drivers/boards/px4fmu/px4fmu_pwm_servo.c @@ -46,13 +46,9 @@ #include <arch/board/board.h> #include <drivers/drv_pwm_output.h> -#include "chip.h" -#include "up_internal.h" -#include "up_arch.h" - -#include "stm32_internal.h" -#include "stm32_gpio.h" -#include "stm32_tim.h" +#include <stm32_internal.h> +#include <stm32_gpio.h> +#include <stm32_tim.h> __EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { { diff --git a/apps/drivers/drv_led.h b/apps/drivers/drv_led.h new file mode 100644 index 000000000..bf21787f2 --- /dev/null +++ b/apps/drivers/drv_led.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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. + * + ****************************************************************************/ + +/** + * @file drv_led.h + * + * LED driver API + */ + +#pragma once + +#include <stdint.h> +#include <sys/ioctl.h> + +#define LED_DEVICE_PATH "/dev/led" + +#define _LED_BASE 0x2800 + +/* PX4 LED colour codes */ +#define LED_AMBER 0 +#define LED_RED 0 /* some boards have red rather than amber */ +#define LED_BLUE 1 + +#define LED_ON _IOC(_LED_BASE, 0) +#define LED_OFF _IOC(_LED_BASE, 1) + +__BEGIN_DECLS + +/* + * Initialise the LED driver. + */ +__EXPORT extern void drv_led_start(); + +__END_DECLS diff --git a/apps/systemcmds/led/Makefile b/apps/drivers/led/Makefile index eb9d8f909..7de188259 100644 --- a/apps/systemcmds/led/Makefile +++ b/apps/drivers/led/Makefile @@ -32,11 +32,7 @@ ############################################################################ # -# Makefile to build ardrone interface +# Makefile to build the LED driver. # -APPNAME = led -PRIORITY = SCHED_PRIORITY_MAX - 15 -STACKSIZE = 2048 - include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/led/led.cpp b/apps/drivers/led/led.cpp new file mode 100644 index 000000000..12d864be2 --- /dev/null +++ b/apps/drivers/led/led.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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. + * + ****************************************************************************/ + +/** + * @file led.cpp + * + * LED driver. + */ + +#include <nuttx/config.h> +#include <drivers/device/device.h> +#include <drivers/drv_led.h> + +/* Ideally we'd be able to get these from up_internal.h */ +//#include <up_internal.h> +__BEGIN_DECLS +extern void up_ledinit(); +extern void up_ledon(int led); +extern void up_ledoff(int led); +__END_DECLS + +class LED : device::CDev +{ +public: + LED(); + ~LED(); + + virtual int init(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); +}; + +LED::LED() : + CDev("led", LED_DEVICE_PATH) +{ + // force immediate init/device registration + init(); +} + +LED::~LED() +{ +} + +int +LED::init() +{ + CDev::init(); + up_ledinit(); + + return 0; +} + +int +LED::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int result = OK; + + switch (cmd) { + case LED_ON: + up_ledon(arg); + break; + + case LED_OFF: + up_ledoff(arg); + break; + + default: + result = CDev::ioctl(filp, cmd, arg); + } + return result; +} + +namespace +{ +LED *gLED; +} + +void +drv_led_start() +{ + if (gLED == nullptr) { + gLED = new LED; + if (gLED != nullptr) + gLED->init(); + } +}
\ No newline at end of file diff --git a/apps/px4/tests/test_float.c b/apps/px4/tests/test_float.c index 8c70b720b..4921c9bbb 100644 --- a/apps/px4/tests/test_float.c +++ b/apps/px4/tests/test_float.c @@ -45,8 +45,6 @@ #include <fcntl.h> #include <errno.h> #include <debug.h> -#include <arch/board/board.h> -#include <arch/board/drv_led.h> #include "tests.h" #include <math.h> #include <float.h> diff --git a/apps/px4/tests/test_int.c b/apps/px4/tests/test_int.c index 40e030641..c59cee7b7 100644 --- a/apps/px4/tests/test_int.c +++ b/apps/px4/tests/test_int.c @@ -49,8 +49,6 @@ #include <arch/board/board.h> -#include <arch/board/drv_led.h> - #include "tests.h" #include <math.h> diff --git a/apps/px4/tests/test_led.c b/apps/px4/tests/test_led.c index 53615ccd8..6e3efc668 100644 --- a/apps/px4/tests/test_led.c +++ b/apps/px4/tests/test_led.c @@ -49,7 +49,7 @@ #include <arch/board/board.h> -#include <arch/board/drv_led.h> +#include <drivers/drv_led.h> #include "tests.h" @@ -91,7 +91,7 @@ int test_led(int argc, char *argv[]) int fd; int ret = 0; - fd = open("/dev/led", O_RDONLY | O_NONBLOCK); + fd = open(LED_DEVICE_PATH, 0); if (fd < 0) { printf("\tLED: open fail\n"); diff --git a/apps/px4/tests/test_sleep.c b/apps/px4/tests/test_sleep.c index 911a9c2e1..c7b9d2833 100644 --- a/apps/px4/tests/test_sleep.c +++ b/apps/px4/tests/test_sleep.c @@ -49,8 +49,6 @@ #include <arch/board/board.h> -#include <arch/board/drv_led.h> - #include "tests.h" /**************************************************************************** diff --git a/apps/px4/tests/test_uart_baudchange.c b/apps/px4/tests/test_uart_baudchange.c index 06965bd3d..609a65c62 100644 --- a/apps/px4/tests/test_uart_baudchange.c +++ b/apps/px4/tests/test_uart_baudchange.c @@ -52,8 +52,6 @@ #include <arch/board/board.h> -#include <arch/board/drv_led.h> - #include "tests.h" #include <math.h> diff --git a/apps/px4/tests/test_uart_console.c b/apps/px4/tests/test_uart_console.c index fc5b03036..f8582b52f 100644 --- a/apps/px4/tests/test_uart_console.c +++ b/apps/px4/tests/test_uart_console.c @@ -50,8 +50,6 @@ #include <arch/board/board.h> -#include <arch/board/drv_led.h> - #include "tests.h" #include <math.h> diff --git a/apps/px4/tests/test_uart_loopback.c b/apps/px4/tests/test_uart_loopback.c index b2e07df1c..3be152004 100644 --- a/apps/px4/tests/test_uart_loopback.c +++ b/apps/px4/tests/test_uart_loopback.c @@ -50,8 +50,6 @@ #include <arch/board/board.h> -#include <arch/board/drv_led.h> - #include "tests.h" #include <math.h> diff --git a/apps/px4/tests/test_uart_send.c b/apps/px4/tests/test_uart_send.c index f5c26e9f3..7e1e8d307 100644 --- a/apps/px4/tests/test_uart_send.c +++ b/apps/px4/tests/test_uart_send.c @@ -50,8 +50,6 @@ #include <arch/board/board.h> -#include <arch/board/drv_led.h> - #include "tests.h" #include <math.h> diff --git a/apps/px4/tests/tests.h b/apps/px4/tests/tests.h index 1023f5f51..46450d10b 100644 --- a/apps/px4/tests/tests.h +++ b/apps/px4/tests/tests.h @@ -84,7 +84,6 @@ extern int test_led(int argc, char *argv[]); extern int test_adc(int argc, char *argv[]); extern int test_int(int argc, char *argv[]); extern int test_float(int argc, char *argv[]); -extern int test_eeproms(int argc, char *argv[]); extern int test_ppm(int argc, char *argv[]); extern int test_servo(int argc, char *argv[]); extern int test_uart_loopback(int argc, char *argv[]); diff --git a/apps/systemcmds/led/led.c b/apps/systemcmds/led/led.c deleted file mode 100644 index 15d448118..000000000 --- a/apps/systemcmds/led/led.c +++ /dev/null @@ -1,209 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * - * 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 led.c - * Plain, stupid led outputs - */ - -#include <nuttx/config.h> -#include <stdio.h> -#include <stdlib.h> -#include <string.h> -#include <stdbool.h> -#include <unistd.h> -#include <math.h> -#include <fcntl.h> -#include <errno.h> -#include <debug.h> -#include <termios.h> -#include <time.h> -#include <sys/prctl.h> -#include <drivers/drv_hrt.h> -#include <arch/board/drv_led.h> - -#include <systemlib/err.h> -#include <systemlib/systemlib.h> - -__EXPORT int 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 led_task; /**< Handle of deamon task / thread */ -static int leds; - -static int led_init(void) -{ - leds = open("/dev/led", O_RDONLY | O_NONBLOCK); - - if (leds < 0) { - errx(1, "[led] LED: open fail\n"); - } - - if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - errx(1, "[led] LED: ioctl fail\n"); - } - - return 0; -} - -static void led_deinit(void) -{ - close(leds); -} - -static int led_toggle(int led) -{ - static int last_blue = LED_ON; - static int last_amber = LED_ON; - - if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; - - if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; - - return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); -} - -static int led_on(int led) -{ - return ioctl(leds, LED_ON, led); -} - -static int led_off(int led) -{ - return ioctl(leds, LED_OFF, led); -} - -/** - * Mainloop of led. - */ -int led_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: led {start|stop|status} [-d <UART>]\n\n"); - exit(1); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int led_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - printf("led already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - led_task = task_spawn("led", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 4096, - led_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - printf("\tled is running\n"); - - } else { - printf("\tled not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int led_thread_main(int argc, char *argv[]) -{ - /* welcome user */ - printf("[led] Control started, taking over motors\n"); - - /* open leds */ - led_init(); - - unsigned int rate = 200; - - while (!thread_should_exit) { - /* swell blue led */ - - - /* 200 Hz base loop */ - usleep(1000000 / rate); - } - - /* close leds */ - led_deinit(); - - printf("[led] ending now...\n\n"); - fflush(stdout); - - thread_running = false; - - return OK; -} - |