diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.c index 5d7186ec1b..a024b5c044 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 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 @@ -40,6 +40,8 @@ */ #include +#include +#include #include #include @@ -49,12 +51,8 @@ #include "dsm.h" #include -#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 -#include -#endif - -#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 -#include +#ifndef GPIO_SPEKTRUM_PWR_EN + #error DSM input driver not supported by this board config #endif #define DSM_FRAME_SIZE 16 /**= 0) { @@ -247,15 +233,34 @@ dsm_init(const char *device) /* reset the format detector */ dsm_guess_format(true); - //debug("DSM: ready"); + ret = 0; + } - } else { + return ret; +} - //debug("DSM: open failed"); +/** + * Initialize the DSM receive functionality + * + * Open the UART for receiving DSM frames and configure it appropriately + * + * @param[in] device Device name of DSM UART + */ +int +dsm_init(const char *device) +{ + if (dsm_fd < 0) { + dsm_fd = open(device, O_RDONLY | O_NONBLOCK); } - return dsm_fd; + int ret = dsm_config(dsm_fd); + + if (!ret) { + return dsm_fd; + } else { + return -1; + } } /** @@ -267,43 +272,32 @@ dsm_init(const char *device) void dsm_bind(uint16_t cmd, int pulses) { -#if !defined(GPIO_USART1_RX_SPEKTRUM) -#else - if (dsm_fd < 0) { return; } switch (cmd) { - case dsm_bind_power_down: + case DSM_CMD_BIND_POWER_DOWN: /*power down DSM satellite*/ -#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) - POWER_RELAY1(0); -#elif defined(CONFIG_ARCH_BOARD_PX4IO_V2) POWER_SPEKTRUM(0); -#endif break; - case dsm_bind_power_up: + case DSM_CMD_BIND_POWER_UP: /*power up DSM satellite*/ -#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) - POWER_RELAY1(1); -#elif defined(CONFIG_ARCH_BOARD_PX4IO_V2) POWER_SPEKTRUM(1); -#endif dsm_guess_format(true); break; - case dsm_bind_set_rx_out: + case DSM_CMD_BIND_SET_RX_OUT: /*Set UART RX pin to active output mode*/ stm32_configgpio(GPIO_USART1_RX_SPEKTRUM); break; - case dsm_bind_send_pulses: + case DSM_CMD_BIND_SEND_PULSES: /*Pulse RX pin a number of times*/ for (int i = 0; i < pulses; i++) { @@ -315,15 +309,13 @@ dsm_bind(uint16_t cmd, int pulses) break; - case dsm_bind_reinit_uart: + case DSM_CMD_BIND_REINIT_UART: /*Restore USART RX pin to RS232 receive mode*/ stm32_configgpio(GPIO_USART1_RX); break; } - -#endif } /** diff --git a/src/lib/rc/dsm.h b/src/lib/rc/dsm.h index 80614d066e..acef60705d 100644 --- a/src/lib/rc/dsm.h +++ b/src/lib/rc/dsm.h @@ -46,7 +46,16 @@ __BEGIN_DECLS __EXPORT int dsm_init(const char *device); +__EXPORT int dsm_config(int dsm_fd); __EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values); __EXPORT void dsm_bind(uint16_t cmd, int pulses); +enum { /* DSM bind states */ + DSM_CMD_BIND_POWER_DOWN = 0, + DSM_CMD_BIND_POWER_UP, + DSM_CMD_BIND_SET_RX_OUT, + DSM_CMD_BIND_SEND_PULSES, + DSM_CMD_BIND_REINIT_UART +} DSM_CMD; + __END_DECLS diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.c index f911648416..e89e8bfe1f 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.c @@ -122,6 +122,20 @@ sbus_init(const char *device, bool singlewire) { int sbus_fd = open(device, O_RDWR | O_NONBLOCK); + int ret = sbus_config(sbus_fd, singlewire); + + if (!ret) { + return sbus_fd; + } else { + return -1; + } +} + +int +sbus_config(int sbus_fd, bool singlewire) +{ + int ret = -1; + if (sbus_fd >= 0) { struct termios t; @@ -133,8 +147,8 @@ sbus_init(const char *device, bool singlewire) if (singlewire) { /* only defined in configs capable of IOCTL */ -#ifdef SBUS_SERIAL_PORT - //ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); +#ifdef TIOCSSINGLEWIRE + ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); #endif } @@ -143,9 +157,11 @@ sbus_init(const char *device, bool singlewire) last_rx_time = hrt_absolute_time(); last_frame_time = last_rx_time; sbus_frame_drops = 0; + + ret = 0; } - return sbus_fd; + return ret; } void diff --git a/src/lib/rc/sbus.h b/src/lib/rc/sbus.h index 0da5ea2935..2c8f389c86 100644 --- a/src/lib/rc/sbus.h +++ b/src/lib/rc/sbus.h @@ -68,6 +68,7 @@ __EXPORT int sbus_init(const char *device, bool singlewire); * provides a degree of protection. Of course, it would be better * if we didn't drop bytes... */ +__EXPORT int sbus_config(int sbus_fd, bool singlewire); __EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);