Browse Source

MindPX: Support serial RC input.

sbg
Henry Zhang 9 years ago committed by Lorenz Meier
parent
commit
d8ac044414
  1. 1
      cmake/configs/nuttx_mindpx-v2_default.cmake
  2. 8
      nuttx-configs/mindpx-v2/include/board.h
  3. 27
      src/drivers/boards/mindpx-v2/board_config.h
  4. 19
      src/drivers/boards/mindpx-v2/mindpx2_init.c

1
cmake/configs/nuttx_mindpx-v2_default.cmake

@ -135,6 +135,7 @@ set(config_module_list @@ -135,6 +135,7 @@ set(config_module_list
lib/controllib
lib/mathlib
lib/mathlib/math/filter
lib/rc
lib/ecl
lib/external_lgpl
lib/geo

8
nuttx-configs/mindpx-v2/include/board.h

@ -199,8 +199,8 @@ @@ -199,8 +199,8 @@
/*
* UARTs.
*/
#define GPIO_USART1_RX GPIO_USART1_RX_1 /* console in from IO */
#define GPIO_USART1_TX 0 /* USART1 is RX-only */
#define GPIO_USART1_RX GPIO_USART1_RX_2 /* RC_INPUT */
#define GPIO_USART1_TX GPIO_USART1_TX_2
#define GPIO_USART2_RX GPIO_USART2_RX_2
#define GPIO_USART2_TX GPIO_USART2_TX_2
@ -215,8 +215,8 @@ @@ -215,8 +215,8 @@
#define GPIO_UART4_RX GPIO_UART4_RX_1
#define GPIO_UART4_TX GPIO_UART4_TX_1
#define GPIO_USART6_RX GPIO_USART6_RX_1
#define GPIO_USART6_TX GPIO_USART6_TX_1
#define GPIO_USART6_RX GPIO_USART6_RX_2
#define GPIO_USART6_TX GPIO_USART6_TX_2
#define GPIO_UART7_RX GPIO_UART7_RX_1
#define GPIO_UART7_TX GPIO_UART7_TX_1

27
src/drivers/boards/mindpx-v2/board_config.h

@ -57,17 +57,6 @@ @@ -57,17 +57,6 @@
****************************************************************************************************/
/* Configuration ************************************************************************************/
/* PX4IO connection configuration */
#define PX4IO_SERIAL_DEVICE "/dev/ttyS4"
#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX
#define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX
#define PX4IO_SERIAL_BASE STM32_USART6_BASE /* hardwired on the board */
#define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6
#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX_2
#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX_2
#define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY
#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */
/* PX4FMU GPIOs ***********************************************************************************/
/* LEDs */
@ -287,11 +276,27 @@ @@ -287,11 +276,27 @@
#define HRT_PPM_CHANNEL 1
#define GPIO_PPM_IN GPIO_TIM8_CH1IN_1
/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */
#define PWMIN_TIMER 3
#define PWMIN_TIMER_CHANNEL 1
#define GPIO_PWM_IN GPIO_TIM3_CH1IN_3
#define RC_SERIAL_PORT "/dev/ttyS0"
#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10)
#define INVERT_RC_INPUT(_s) px4_arch_gpiowrite(GPIO_SBUS_INV, _s);
/* Power switch controls */
#define GPIO_SPEKTRUM_PWR_EN
#define POWER_SPEKTRUM(_s) do { } while (0)
#define SPEKTRUM_RX_AS_UART() px4_arch_configgpio(GPIO_USART1_RX)
/* MindPXv2 has a separate GPIO for serial RC output */
#define GPIO_RC_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN6)
#define SPEKTRUM_RX_AS_GPIO() px4_arch_configgpio(GPIO_RC_OUT)
#define SPEKTRUM_RX_HIGH(_s) px4_arch_gpiowrite(GPIO_RC_OUT, (_s))
#define BOARD_NAME "MINDPX_V2"
/* By Providing BOARD_ADC_USB_CONNECTED this board support the ADC

19
src/drivers/boards/mindpx-v2/mindpx2_init.c

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015, 2016 Airmind Development Team. All rights reserved.
* Copyright (c) 2015, 2016 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
@ -12,7 +12,7 @@ @@ -12,7 +12,7 @@
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name Airmind nor the names of its contributors may be
* 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.
*
@ -177,12 +177,15 @@ __EXPORT int nsh_archinitialize(void) @@ -177,12 +177,15 @@ __EXPORT int nsh_archinitialize(void)
px4_arch_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */
/* configure power supply control/sense pins */
// px4_arch_configgpio(GPIO_VDD_5V_PERIPH_EN);
// px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN);
// px4_arch_configgpio(GPIO_VDD_BRICK_VALID);
// px4_arch_configgpio(GPIO_VDD_SERVO_VALID);
// px4_arch_configgpio(GPIO_VDD_5V_HIPOWER_OC);
// px4_arch_configgpio(GPIO_VDD_5V_PERIPH_OC);
// px4_arch_configgpio(GPIO_VDD_5V_PERIPH_EN);
// px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN);
// px4_arch_configgpio(GPIO_VDD_BRICK_VALID);
// px4_arch_configgpio(GPIO_VDD_SERVO_VALID);
// px4_arch_configgpio(GPIO_VDD_5V_HIPOWER_OC);
// px4_arch_configgpio(GPIO_VDD_5V_PERIPH_OC);
px4_arch_configgpio(GPIO_SBUS_INV);
px4_arch_configgpio(GPIO_RC_OUT); /* Serial RC output pin */
px4_arch_gpiowrite(GPIO_RC_OUT, 1); /* set it high to pull RC input up */
/* configure the high-resolution time/callout interface */
hrt_init();

Loading…
Cancel
Save