Browse Source

boards: simplify RC port configuration by using NuttX ioctl's

A board only needs to define:
 #define RC_SERIAL_PORT                     "/dev/ttyS4"

Then it can optionally define one or more of the following:
 #define RC_SERIAL_SWAP_RXTX
 #define RC_SERIAL_SINGLEWIRE
 #define RC_INVERT_INPUT(_invert_true) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true)
sbg
Beat Küng 6 years ago
parent
commit
b7a0e1ef03
  1. 2
      boards/airmind/mindpx-v2/src/board_config.h
  2. 3
      boards/av/x-v1/nuttx-config/nsh/defconfig
  3. 1
      boards/av/x-v1/src/CMakeLists.txt
  4. 5
      boards/av/x-v1/src/board_config.h
  5. 45
      boards/av/x-v1/src/init.c
  6. 67
      boards/av/x-v1/src/manifest.c
  7. 1
      boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig
  8. 1
      boards/nxp/fmuk66-v3/src/board_config.h
  9. 35
      boards/nxp/fmuk66-v3/src/init.c
  10. 2
      boards/px4/fmu-v4/src/board_config.h
  11. 1
      boards/px4/fmu-v4pro/src/board_config.h
  12. 2
      boards/px4/fmu-v5/nuttx-config/nsh/defconfig
  13. 6
      boards/px4/fmu-v5/src/board_config.h
  14. 45
      boards/px4/fmu-v5/src/init.c
  15. 14
      boards/px4/fmu-v5/src/manifest.c
  16. 103
      src/drivers/boards/common/board_common.h
  17. 107
      src/drivers/rc_input/RCInput.cpp
  18. 6
      src/drivers/rc_input/RCInput.hpp

2
boards/airmind/mindpx-v2/src/board_config.h

@ -271,7 +271,7 @@ @@ -271,7 +271,7 @@
// #define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10)
#define BOARD_INVERT_RC_INPUT(_invert_true, _na) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true);
#define RC_INVERT_INPUT(_invert_true) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true);
#define GPIO_FRSKY_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN12)
#define INVERT_FRSKY(_invert_true) px4_arch_gpiowrite(GPIO_FRSKY_INV, _invert_true);

3
boards/av/x-v1/nuttx-config/nsh/defconfig

@ -226,6 +226,9 @@ CONFIG_STM32F7_USART2=y @@ -226,6 +226,9 @@ CONFIG_STM32F7_USART2=y
CONFIG_STM32F7_USART3=y
CONFIG_STM32F7_USART6=y
CONFIG_STM32F7_USART_BREAKS=y
CONFIG_STM32F7_USART_INVERT=y
CONFIG_STM32F7_USART_SINGLEWIRE=y
CONFIG_STM32F7_USART_SWAP=y
CONFIG_STM32F7_WWDG=y
CONFIG_SYSTEM_NSH=y
CONFIG_SYSTEM_PING=y

1
boards/av/x-v1/src/CMakeLists.txt

@ -33,7 +33,6 @@ @@ -33,7 +33,6 @@
add_library(drivers_board
init.c
manifest.c
sdio.c
spi.cpp
timer_config.c

5
boards/av/x-v1/src/board_config.h

@ -268,12 +268,7 @@ @@ -268,12 +268,7 @@
/* RC Serial port */
#define RC_UXART_BASE STM32_UART5_BASE
#define RC_SERIAL_PORT "/dev/ttyS4"
#define BOARD_HAS_SINGLE_WIRE 0 /* HW is capable of Single Wire */
#define BOARD_HAS_SINGLE_WIRE_ON_TX 0 /* HW default is wired as Single Wire On TX pin */
#define BOARD_HAS_RX_TX_SWAP 0 /* HW Can swap TX and RX */
#define RC_SERIAL_PORT_IS_SWAPED 0 /* Board wired with RC's TX is on cpu RX */
/* Power switch controls ******************************************************/

45
boards/av/x-v1/src/init.c

@ -75,51 +75,6 @@ @@ -75,51 +75,6 @@
static int configure_switch(void);
/************************************************************************************
* Name: board_rc_input
*
* Description:
* All boards my optionally provide this API to invert the Serial RC input.
* This is needed on SoCs that support the notion RXINV or TXINV as apposed to
* and external XOR controlled by a GPIO
*
************************************************************************************/
__EXPORT void board_rc_input(bool invert_on, uint32_t uxart_base)
{
irqstate_t irqstate = px4_enter_critical_section();
uint32_t cr1 = getreg32(STM32_USART_CR1_OFFSET + uxart_base);
uint32_t cr2 = getreg32(STM32_USART_CR2_OFFSET + uxart_base);
uint32_t regval = cr1;
/* {R|T}XINV bit fields can only be written when the USART is disabled (UE=0). */
regval &= ~USART_CR1_UE;
putreg32(regval, STM32_USART_CR1_OFFSET + uxart_base);
if (invert_on) {
#if defined(BOARD_HAS_RX_TX_SWAP) && RC_SERIAL_PORT_IS_SWAPED == 1
/* This is only ever turned on */
cr2 |= (USART_CR2_RXINV | USART_CR2_TXINV | USART_CR2_SWAP);
#else
cr2 |= (USART_CR2_RXINV | USART_CR2_TXINV);
#endif
} else {
cr2 &= ~(USART_CR2_RXINV | USART_CR2_TXINV);
}
putreg32(cr2, STM32_USART_CR2_OFFSET + uxart_base);
putreg32(cr1, STM32_USART_CR1_OFFSET + uxart_base);
leave_critical_section(irqstate);
}
/************************************************************************************
* Name: board_on_reset
*

67
boards/av/x-v1/src/manifest.c

@ -1,67 +0,0 @@ @@ -1,67 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 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 manifest.c
*
* This module supplies the interface to the manifest of hardware that is
* optional and dependent on the HW REV and HW VER IDs
*
* The manifest allows the system to know whether a hardware option
* say for example the PX4IO is an no-pop option vs it is broken.
*
*/
/****************************************************************************
* Included Files
****************************************************************************/
#include <nuttx/config.h>
#include <board_config.h>
#include <stdbool.h>
#include "systemlib/px4_macros.h"
/************************************************************************************
* Name: board_rc_input
*
* Description:
* All boards my optionally provide this API to invert the Serial RC input.
* This is needed on SoCs that support the notion RXINV or TXINV as opposed to
* and external XOR controlled by a GPIO
*
************************************************************************************/
__EXPORT bool board_supports_single_wire(uint32_t uxart_base)
{
return uxart_base == RC_UXART_BASE;
}

1
boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig

@ -89,6 +89,7 @@ CONFIG_KINETIS_UART4_RXDMA=y @@ -89,6 +89,7 @@ CONFIG_KINETIS_UART4_RXDMA=y
CONFIG_KINETIS_UARTFIFOS=y
CONFIG_KINETIS_UART_BREAKS=y
CONFIG_KINETIS_UART_EXTEDED_BREAK=y
CONFIG_KINETIS_UART_INVERT=y
CONFIG_KINETIS_USBDCD=y
CONFIG_KINETS_LPUART_LOWEST=y
CONFIG_LIBC_FLOATINGPOINT=y

1
boards/nxp/fmuk66-v3/src/board_config.h

@ -130,7 +130,6 @@ __BEGIN_DECLS @@ -130,7 +130,6 @@ __BEGIN_DECLS
/* RC input */
#define RC_UXART_BASE KINETIS_UART1_BASE
#define RC_SERIAL_PORT "/dev/ttyS2" /* UART1 */
#define GPIO_RSSI_IN PIN_ADC1_SE13

35
boards/nxp/fmuk66-v3/src/init.c

@ -140,41 +140,6 @@ int board_read_VBUS_state(void) @@ -140,41 +140,6 @@ int board_read_VBUS_state(void)
return BOARD_ADC_USB_CONNECTED ? 0 : 1;
}
/************************************************************************************
* Name: board_rc_input
*
* Description:
* All boards my optionally provide this API to invert the Serial RC input.
* This is needed on SoCs that support the notion RXINV or TXINV as opposed to
* and external XOR controlled by a GPIO
*
************************************************************************************/
__EXPORT void board_rc_input(bool invert_on, uint32_t uxart_base)
{
irqstate_t irqstate = px4_enter_critical_section();
uint8_t s2 = getreg8(KINETIS_UART_S2_OFFSET + uxart_base);
uint8_t c3 = getreg8(KINETIS_UART_C3_OFFSET + uxart_base);
/* {R|T}XINV bit fields can written any time */
if (invert_on) {
s2 |= (UART_S2_RXINV);
c3 |= (UART_C3_TXINV);
} else {
s2 &= ~(UART_S2_RXINV);
c3 &= ~(UART_C3_TXINV);
}
putreg8(s2, KINETIS_UART_S2_OFFSET + uxart_base);
putreg8(c3, KINETIS_UART_C3_OFFSET + uxart_base);
leave_critical_section(irqstate);
}
/************************************************************************************
* Name: board_peripheral_reset
*

2
boards/px4/fmu-v4/src/board_config.h

@ -306,7 +306,7 @@ @@ -306,7 +306,7 @@
/* For R12, this signal is active high. */
#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define BOARD_INVERT_RC_INPUT(_invert_true, _na) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true)
#define RC_INVERT_INPUT(_invert_true) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true)
#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)

1
boards/px4/fmu-v4pro/src/board_config.h

@ -306,7 +306,6 @@ @@ -306,7 +306,6 @@
*/
#define GPIO_BTN_SAFETY_FMU (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTC|GPIO_PIN4)
#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define BOARD_INVERT_RC_INPUT(_invert_true, _na) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true)
#define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2)
//TODO: fo not see on schematic #define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)

2
boards/px4/fmu-v5/nuttx-config/nsh/defconfig

@ -205,7 +205,9 @@ CONFIG_STM32F7_USART2=y @@ -205,7 +205,9 @@ CONFIG_STM32F7_USART2=y
CONFIG_STM32F7_USART3=y
CONFIG_STM32F7_USART6=y
CONFIG_STM32F7_USART_BREAKS=y
CONFIG_STM32F7_USART_INVERT=y
CONFIG_STM32F7_USART_SINGLEWIRE=y
CONFIG_STM32F7_USART_SWAP=y
CONFIG_STM32F7_WWDG=y
CONFIG_SYSTEM_CDCACM=y
CONFIG_SYSTEM_NSH=y

6
boards/px4/fmu-v5/src/board_config.h

@ -496,12 +496,8 @@ @@ -496,12 +496,8 @@
/* RC Serial port */
#define RC_UXART_BASE STM32_USART6_BASE
#define RC_SERIAL_PORT "/dev/ttyS4"
#define BOARD_HAS_SINGLE_WIRE 1 /* HW is capable of Single Wire */
#define BOARD_HAS_SINGLE_WIRE_ON_TX 1 /* HW default is wired as Single Wire On TX pin */
#define BOARD_HAS_RX_TX_SWAP 1 /* HW Can swap TX and RX */
#define RC_SERIAL_PORT_IS_SWAPED 0 /* Board wired with RC's TX is on cpu RX */
#define RC_SERIAL_SINGLEWIRE
/* Input Capture Channels. */
#define INPUT_CAP1_TIMER 2

45
boards/px4/fmu-v5/src/init.c

@ -91,51 +91,6 @@ extern void led_off(int led); @@ -91,51 +91,6 @@ extern void led_off(int led);
__END_DECLS
/************************************************************************************
* Name: board_rc_input
*
* Description:
* All boards my optionally provide this API to invert the Serial RC input.
* This is needed on SoCs that support the notion RXINV or TXINV as apposed to
* and external XOR controlled by a GPIO
*
************************************************************************************/
__EXPORT void board_rc_input(bool invert_on, uint32_t uxart_base)
{
irqstate_t irqstate = px4_enter_critical_section();
uint32_t cr1 = getreg32(STM32_USART_CR1_OFFSET + uxart_base);
uint32_t cr2 = getreg32(STM32_USART_CR2_OFFSET + uxart_base);
uint32_t regval = cr1;
/* {R|T}XINV bit fields can only be written when the USART is disabled (UE=0). */
regval &= ~USART_CR1_UE;
putreg32(regval, STM32_USART_CR1_OFFSET + uxart_base);
if (invert_on) {
#if defined(BOARD_HAS_RX_TX_SWAP) && RC_SERIAL_PORT_IS_SWAPED == 1
/* This is only ever turned on */
cr2 |= (USART_CR2_RXINV | USART_CR2_TXINV | USART_CR2_SWAP);
#else
cr2 |= (USART_CR2_RXINV | USART_CR2_TXINV);
#endif
} else {
cr2 &= ~(USART_CR2_RXINV | USART_CR2_TXINV);
}
putreg32(cr2, STM32_USART_CR2_OFFSET + uxart_base);
putreg32(cr1, STM32_USART_CR1_OFFSET + uxart_base);
leave_critical_section(irqstate);
}
/************************************************************************************
* Name: board_peripheral_reset
*

14
boards/px4/fmu-v5/src/manifest.c

@ -96,20 +96,6 @@ static px4_hw_mft_list_entry_t mft_lists[] = { @@ -96,20 +96,6 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
};
/************************************************************************************
* Name: board_rc_input
*
* Description:
* All boards my optionally provide this API to invert the Serial RC input.
* This is needed on SoCs that support the notion RXINV or TXINV as opposed to
* and external XOR controlled by a GPIO
*
************************************************************************************/
__EXPORT bool board_supports_single_wire(uint32_t uxart_base)
{
return uxart_base == RC_UXART_BASE;
}
/************************************************************************************
* Name: board_query_manifest
*

103
src/drivers/boards/common/board_common.h

@ -46,6 +46,7 @@ @@ -46,6 +46,7 @@
#include <errno.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
/************************************************************************************
* Definitions
************************************************************************************/
@ -453,101 +454,101 @@ typedef uint8_t px4_guid_t[PX4_GUID_BYTE_LENGTH]; @@ -453,101 +454,101 @@ typedef uint8_t px4_guid_t[PX4_GUID_BYTE_LENGTH];
************************************************************************************/
__BEGIN_DECLS
/* Provide an interface for determining if a board supports single wire */
/************************************************************************************
* Name: board_supports_single_wire
* Name: board_rc_singlewire
*
* Description:
* A board may provide serial ports that supports single wire.
* This interface will call into the board support code to determine
* if the interface is available at runtime, on this version of the
* hardware.
* A board may define RC_SERIAL_SINGLEWIRE, so that RC_SERIAL_PORT is configured
* as singlewire UART.
*
* Input Parameters:
* uxart_base - the base address of the UxART.
* device: serial device, e.g. "/dev/ttyS0"
*
* Returned Value:
* true the hardware supports this interface.
* true singlewire should be enabled.
* false if not.
*
************************************************************************************/
#if !defined(BOARD_HAS_SINGLE_WIRE)
# define board_supports_single_wire(_uxart_base) false
#if defined(RC_SERIAL_SINGLEWIRE)
static inline bool board_rc_singlewire(const char *device) { return strcmp(device, RC_SERIAL_PORT) == 0; }
#else
__EXPORT bool board_supports_single_wire(uint32_t uxart_base);
static inline bool board_rc_singlewire(const char *device) { return false; }
#endif
/* Provide an interface for reading the connected state of VBUS */
/************************************************************************************
* Name: board_read_VBUS_state
* Name: board_rc_swap_rxtx
*
* Description:
* All boards must provide a way to read the state of VBUS, this my be simple
* digital input on a GPIO. Or something more complicated like a Analong input
* or reading a bit from a USB controller register.
* A board may define RC_SERIAL_SWAP_RXTX, so that RC_SERIAL_PORT is configured
* as UART with RX/TX swapped.
*
* Input Parameters:
* None
* device: serial device, e.g. "/dev/ttyS0"
*
* Returned Value:
* 0 if connected.
* true RX/RX should be swapped.
* false if not.
*
************************************************************************************/
#if defined(GPIO_OTGFS_VBUS)
# define board_read_VBUS_state() (px4_arch_gpioread(GPIO_OTGFS_VBUS) ? 0 : 1)
#if defined(RC_SERIAL_SWAP_RXTX)
static inline bool board_rc_swap_rxtx(const char *device) { return strcmp(device, RC_SERIAL_PORT) == 0; }
#else
int board_read_VBUS_state(void);
static inline bool board_rc_swap_rxtx(const char *device) { return false; }
#endif
/************************************************************************************
* Name: board_rc_input
* Name: board_rc_invert_input
*
* Description:
* All boards my optionally provide this API to invert the Serial RC input.
* This is needed on SoCs that support the notion RXINV or TXINV as opposed to
* and external XOR controlled by a GPIO
* All boards may optionally define RC_INVERT_INPUT(bool invert) that is
* used to invert the RC_SERIAL_PORT RC port (e.g. to toggle an external XOR via
* GPIO).
*
* Input Parameters:
* invert_on - A positive logic value, that when true (on) will set the HW in
* inverted NRZ mode where a MARK will be 0 and SPACE will be a 1.
*
* Returned Value:
* None
* true the UART inversion got set.
*
************************************************************************************/
/* Provide an interface for Inversion of serial data
*
* Case 1:Board does provide UxART based inversion
* Use it, and it will define RC_UXART_BASE
*
* Case 1:Board does provide GPIO inversion
* Use it and let board determine active state
* Define RC_UXART_BASE as empty
*
* Case 3:Board does not provide any inversions
* Default to nop
* Define RC_UXART_BASE as empty
*/
#ifdef RC_INVERT_INPUT
static inline bool board_rc_invert_input(const char *device, bool invert)
{
if (strcmp(device, RC_SERIAL_PORT) == 0) { RC_INVERT_INPUT(invert); return true; }
#if defined(RC_UXART_BASE)
__EXPORT void board_rc_input(bool invert_on, uint32_t uxart_base);
# define INVERT_RC_INPUT(_invert_true, _rc_uxart) board_rc_input((_invert_true), (_rc_uxart));
return false;
}
#else
static inline bool board_rc_invert_input(const char *device, bool invert) { return false; }
#endif
#if defined(BOARD_INVERT_RC_INPUT)
# define INVERT_RC_INPUT BOARD_INVERT_RC_INPUT
# define RC_UXART_BASE 0
#endif
/* Provide an interface for reading the connected state of VBUS */
#if !defined(INVERT_RC_INPUT)
# define INVERT_RC_INPUT(_invert_true, _na) while(0)
# define RC_UXART_BASE 0
/************************************************************************************
* Name: board_read_VBUS_state
*
* Description:
* All boards must provide a way to read the state of VBUS, this my be simple
* digital input on a GPIO. Or something more complicated like a Analong input
* or reading a bit from a USB controller register.
*
* Input Parameters:
* None
*
* Returned Value:
* 0 if connected.
*
************************************************************************************/
#if defined(GPIO_OTGFS_VBUS)
# define board_read_VBUS_state() (px4_arch_gpioread(GPIO_OTGFS_VBUS) ? 0 : 1)
#else
int board_read_VBUS_state(void);
#endif
/************************************************************************************

107
src/drivers/rc_input/RCInput.cpp

@ -44,7 +44,7 @@ static bool bind_spektrum(int arg); @@ -44,7 +44,7 @@ static bool bind_spektrum(int arg);
work_s RCInput::_work = {};
constexpr char const *RCInput::RC_SCAN_STRING[];
RCInput::RCInput(bool run_as_task) :
RCInput::RCInput(bool run_as_task, char *device) :
_cycle_perf(perf_alloc(PC_ELAPSED, "rc_input cycle time")),
_publish_interval_perf(perf_alloc(PC_INTERVAL, "rc_input publish interval"))
{
@ -58,6 +58,15 @@ RCInput::RCInput(bool run_as_task) : @@ -58,6 +58,15 @@ RCInput::RCInput(bool run_as_task) :
for (unsigned i = 0; i < input_rc_s::RC_INPUT_MAX_CHANNELS; i++) {
_raw_rc_values[i] = UINT16_MAX;
}
#ifdef RC_SERIAL_PORT
if (device) {
strncpy(_device, device, sizeof(_device));
_device[sizeof(_device) - 1] = '\0';
}
#endif
}
RCInput::~RCInput()
@ -87,11 +96,20 @@ RCInput::init() @@ -87,11 +96,20 @@ RCInput::init()
# endif
// dsm_init sets some file static variables and returns a file descriptor
_rcs_fd = dsm_init(RC_SERIAL_PORT);
_rcs_fd = dsm_init(_device);
if (_rcs_fd < 0) {
return -errno;
}
if (board_rc_swap_rxtx(_device)) {
ioctl(_rcs_fd, TIOCSSWAP, SER_SWAP_ENABLED);
}
// assume SBUS input and immediately switch it to
// so that if Single wire mode on TX there will be only
// a short contention
sbus_config(_rcs_fd, board_supports_single_wire(RC_UXART_BASE));
sbus_config(_rcs_fd, board_rc_singlewire(_device));
# ifdef GPIO_PPM_IN
// disable CPPM input by mapping it away from the timer capture input
px4_arch_unconfiggpio(GPIO_PPM_IN);
@ -110,13 +128,18 @@ RCInput::task_spawn(int argc, char *argv[]) @@ -110,13 +128,18 @@ RCInput::task_spawn(int argc, char *argv[])
int myoptind = 1;
int ch;
const char *myoptarg = nullptr;
const char *device = RC_SERIAL_PORT;
while ((ch = px4_getopt(argc, argv, "t", &myoptind, &myoptarg)) != EOF) {
while ((ch = px4_getopt(argc, argv, "td:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 't':
run_as_task = true;
break;
case 'd':
device = myoptarg;
break;
case '?':
error_flag = true;
break;
@ -136,24 +159,28 @@ RCInput::task_spawn(int argc, char *argv[]) @@ -136,24 +159,28 @@ RCInput::task_spawn(int argc, char *argv[])
if (!run_as_task) {
/* schedule a cycle to start things */
int ret = work_queue(HPWORK, &_work, (worker_t)&RCInput::cycle_trampoline, nullptr, 0);
int ret = work_queue(HPWORK, &_work, (worker_t)&RCInput::cycle_trampoline_init, (void *)device, 0);
if (ret < 0) {
return ret;
}
// we need to wait, otherwise 'device' could go out of scope while still being accessed
wait_until_running();
_task_id = task_id_is_work_queue;
} else {
/* start the IO interface task */
const char *const args[] = { device, nullptr };
_task_id = px4_task_spawn_cmd("rc_input",
SCHED_DEFAULT,
SCHED_PRIORITY_SLOW_DRIVER,
1000,
(px4_main_t)&run_trampoline,
nullptr);
(char *const *)args);
if (_task_id < 0) {
_task_id = -1;
@ -165,28 +192,31 @@ RCInput::task_spawn(int argc, char *argv[]) @@ -165,28 +192,31 @@ RCInput::task_spawn(int argc, char *argv[])
}
void
RCInput::cycle_trampoline(void *arg)
RCInput::cycle_trampoline_init(void *arg)
{
RCInput *dev = reinterpret_cast<RCInput *>(arg);
RCInput *dev = new RCInput(false, (char *)arg);
// check if the trampoline is called for the first time
if (!dev) {
dev = new RCInput(false);
if (!dev) {
PX4_ERR("alloc failed");
return;
}
PX4_ERR("alloc failed");
return;
}
if (dev->init() != 0) {
PX4_ERR("init failed");
delete dev;
return;
}
int ret = dev->init();
_object.store(dev);
if (ret != 0) {
PX4_ERR("init failed (%i)", ret);
delete dev;
return;
}
_object.store(dev);
dev->cycle();
}
void
RCInput::cycle_trampoline(void *arg)
{
RCInput *dev = reinterpret_cast<RCInput *>(arg);
dev->cycle();
}
@ -263,17 +293,23 @@ void RCInput::set_rc_scan_state(RC_SCAN newState) @@ -263,17 +293,23 @@ void RCInput::set_rc_scan_state(RC_SCAN newState)
_rc_scan_state = newState;
}
void RCInput::rc_io_invert(bool invert, uint32_t uxart_base)
void RCInput::rc_io_invert(bool invert)
{
INVERT_RC_INPUT(invert, uxart_base);
// First check if the board provides a board-specific inversion method (e.g. via GPIO),
// and if not use an IOCTL
if (!board_rc_invert_input(_device, invert)) {
ioctl(_rcs_fd, TIOCSINVERT, invert ? (SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX) : 0);
}
}
#endif
void
RCInput::run()
{
if (init() != 0) {
PX4_ERR("init failed");
int ret = init();
if (ret != 0) {
PX4_ERR("init failed (%i)", ret);
exit_and_cleanup();
return;
}
@ -386,8 +422,8 @@ RCInput::cycle() @@ -386,8 +422,8 @@ RCInput::cycle()
if (_rc_scan_begin == 0) {
_rc_scan_begin = cycle_timestamp;
// Configure serial port for SBUS
sbus_config(_rcs_fd, board_supports_single_wire(RC_UXART_BASE));
rc_io_invert(true, RC_UXART_BASE);
sbus_config(_rcs_fd, board_rc_singlewire(_device));
rc_io_invert(true);
} else if (_rc_scan_locked
|| cycle_timestamp - _rc_scan_begin < rc_scan_max) {
@ -418,7 +454,7 @@ RCInput::cycle() @@ -418,7 +454,7 @@ RCInput::cycle()
_rc_scan_begin = cycle_timestamp;
// // Configure serial port for DSM
dsm_config(_rcs_fd);
rc_io_invert(false, RC_UXART_BASE);
rc_io_invert(false);
} else if (_rc_scan_locked
|| cycle_timestamp - _rc_scan_begin < rc_scan_max) {
@ -451,7 +487,7 @@ RCInput::cycle() @@ -451,7 +487,7 @@ RCInput::cycle()
_rc_scan_begin = cycle_timestamp;
// Configure serial port for DSM
dsm_config(_rcs_fd);
rc_io_invert(false, RC_UXART_BASE);
rc_io_invert(false);
} else if (_rc_scan_locked
|| cycle_timestamp - _rc_scan_begin < rc_scan_max) {
@ -499,7 +535,7 @@ RCInput::cycle() @@ -499,7 +535,7 @@ RCInput::cycle()
_rc_scan_begin = cycle_timestamp;
// Configure serial port for DSM
dsm_config(_rcs_fd);
rc_io_invert(false, RC_UXART_BASE);
rc_io_invert(false);
} else if (_rc_scan_locked
|| cycle_timestamp - _rc_scan_begin < rc_scan_max) {
@ -541,7 +577,8 @@ RCInput::cycle() @@ -541,7 +577,8 @@ RCInput::cycle()
_rc_scan_begin = cycle_timestamp;
// Configure timer input pin for CPPM
px4_arch_configgpio(GPIO_PPM_IN);
rc_io_invert(false, RC_UXART_BASE);
rc_io_invert(false);
ioctl(_rcs_fd, TIOCSINVERT, 0);
} else if (_rc_scan_locked || cycle_timestamp - _rc_scan_begin < rc_scan_max) {
@ -575,7 +612,7 @@ RCInput::cycle() @@ -575,7 +612,7 @@ RCInput::cycle()
_rc_scan_begin = cycle_timestamp;
// Configure serial port for CRSF
crsf_config(_rcs_fd);
rc_io_invert(false, RC_UXART_BASE);
rc_io_invert(false);
} else if (_rc_scan_locked
|| cycle_timestamp - _rc_scan_begin < rc_scan_max) {
@ -707,7 +744,7 @@ bool bind_spektrum(int arg) @@ -707,7 +744,7 @@ bool bind_spektrum(int arg)
RCInput *RCInput::instantiate(int argc, char *argv[])
{
// No arguments to parse. We also know that we should run as task
return new RCInput(true);
return new RCInput(true, argv[0]);
}
int RCInput::custom_command(int argc, char *argv[])
@ -760,6 +797,7 @@ When running on the work queue, it schedules at a fixed frequency. @@ -760,6 +797,7 @@ When running on the work queue, it schedules at a fixed frequency.
PRINT_MODULE_USAGE_NAME("rc_input", "driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task (without any mode set, use any of the mode_* cmds)");
PRINT_MODULE_USAGE_PARAM_FLAG('t', "Run as separate task instead of the work queue", true);
PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "<file:dev>", "RC device", true);
#if defined(SPEKTRUM_POWER)
PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "Send a DSM bind command (module must be running)");
@ -777,6 +815,9 @@ int RCInput::print_status() @@ -777,6 +815,9 @@ int RCInput::print_status()
if (!_run_as_task) {
PX4_INFO("Max update rate: %i Hz", 1000000 / _current_update_interval);
}
if (_device[0] != '\0') {
PX4_INFO("Serial device: %s", _device);
}
PX4_INFO("RC scan state: %s, locked: %s", RC_SCAN_STRING[_rc_scan_state], _rc_scan_locked ? "yes" : "no");
PX4_INFO("CRSF Telemetry: %s", _crsf_telemetry ? "yes" : "no");

6
src/drivers/rc_input/RCInput.hpp

@ -63,7 +63,7 @@ class RCInput : public ModuleBase<RCInput> @@ -63,7 +63,7 @@ class RCInput : public ModuleBase<RCInput>
{
public:
RCInput(bool run_as_task);
RCInput(bool run_as_task, char *device);
virtual ~RCInput();
/** @see ModuleBase */
@ -132,6 +132,7 @@ private: @@ -132,6 +132,7 @@ private:
orb_advert_t _to_input_rc{nullptr};
int _rcs_fd{-1};
char _device[20] {}; ///< device / serial port path
uint8_t _rcs_buf[SBUS_BUFFER_SIZE] {};
@ -144,6 +145,7 @@ private: @@ -144,6 +145,7 @@ private:
perf_counter_t _publish_interval_perf;
static void cycle_trampoline(void *arg);
static void cycle_trampoline_init(void *arg);
int start();
void fill_rc_in(uint16_t raw_rc_count_local,
@ -153,6 +155,6 @@ private: @@ -153,6 +155,6 @@ private:
void set_rc_scan_state(RC_SCAN _rc_scan_state);
void rc_io_invert(bool invert, uint32_t uxart_base);
void rc_io_invert(bool invert);
};

Loading…
Cancel
Save