From a3eeafebeb3821806b88c08e0c9957e05a53f575 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 27 Nov 2015 21:31:00 +0100 Subject: [PATCH] Enable UART6 and enable S.BUS. Tested to work. --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + nuttx-configs/px4fmu-v4/include/board.h | 4 +++ nuttx-configs/px4fmu-v4/nsh/defconfig | 2 +- src/drivers/boards/px4fmu-v4/board_config.h | 4 +-- src/drivers/px4fmu/fmu.cpp | 25 +++++++++++-------- .../attitude_estimator_q_main.cpp | 4 +++ src/modules/sensors/sensors.cpp | 2 +- 7 files changed, 27 insertions(+), 15 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index ecfeaa36a9..858c95dd05 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -58,6 +58,7 @@ set(config_module_list systemcmds/mtd systemcmds/dumpfile systemcmds/ver + systemcmds/tests # # General system control diff --git a/nuttx-configs/px4fmu-v4/include/board.h b/nuttx-configs/px4fmu-v4/include/board.h index 54e21bbfe7..2f7c1189f3 100755 --- a/nuttx-configs/px4fmu-v4/include/board.h +++ b/nuttx-configs/px4fmu-v4/include/board.h @@ -215,6 +215,9 @@ #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_UART7_RX GPIO_UART7_RX_1 #define GPIO_UART7_TX GPIO_UART7_TX_1 @@ -222,6 +225,7 @@ /* UART RX DMA configurations */ #define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 /* * CAN diff --git a/nuttx-configs/px4fmu-v4/nsh/defconfig b/nuttx-configs/px4fmu-v4/nsh/defconfig index 96cd833d45..cfe3de4403 100644 --- a/nuttx-configs/px4fmu-v4/nsh/defconfig +++ b/nuttx-configs/px4fmu-v4/nsh/defconfig @@ -258,7 +258,7 @@ CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y CONFIG_STM32_UART4=y # CONFIG_STM32_UART5 is not set -# CONFIG_STM32_USART6 is not set +CONFIG_STM32_USART6=y CONFIG_STM32_UART7=y CONFIG_STM32_UART8=y # CONFIG_STM32_IWDG is not set diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 9a89a7e962..ab94498514 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -214,7 +214,7 @@ __BEGIN_DECLS #define HRT_PPM_CHANNEL 3 /* use capture/compare channel 2 */ #define GPIO_PPM_IN (GPIO_ALT|GPIO_AF2|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) -#define SBUS_SERIAL_PORT "/dev/ttyS4" /* XXX not vetted */ +#define SBUS_SERIAL_PORT "/dev/ttyS4" /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ #define PWMIN_TIMER 4 @@ -225,7 +225,7 @@ __BEGIN_DECLS #define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3) #define GPIO_SAFETY_SWITCH_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) #define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN5) -#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) #define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2) #define GPIO_SPEKTRUM_POWER (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 452cf1fc2c..fbf97c9b68 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -327,12 +327,9 @@ PX4FMU::PX4FMU() : _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; #endif -#ifdef SBUS_SERIAL_PORT - _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); -#endif - -#ifdef DSM_SERIAL_PORT - _dsm_fd = dsm_init(DSM_SERIAL_PORT); +#ifdef GPIO_SBUS_INV + // this board has a GPIO to control SBUS inversion + stm32_configgpio(GPIO_SBUS_INV); #endif /* only enable this during development */ @@ -648,6 +645,15 @@ PX4FMU::cycle() update_pwm_rev_mask(); +#ifdef SBUS_SERIAL_PORT + _sbus_fd = sbus_init(SBUS_SERIAL_PORT, true); +#endif + + #ifdef DSM_SERIAL_PORT + // XXX rather than opening it we need to cycle between protocols until one is locked in + //_dsm_fd = dsm_init(DSM_SERIAL_PORT); + #endif + _initialized = true; } @@ -839,14 +845,11 @@ PX4FMU::cycle() } #endif -#ifdef DSM_SERIAL_PORT - //_dsm_fd = dsm_init(DSM_SERIAL_PORT); -#endif - #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data - if (ppm_last_valid_decode != _rc_in.timestamp_last_signal) { + if ((ppm_last_valid_decode != _rc_in.timestamp_last_signal) && + ppm_decoded_channels > 3) { // we have a new PPM frame. Publish it. _rc_in.channel_count = ppm_decoded_channels; diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index d069786699..8ca0c74e32 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -206,6 +206,8 @@ private: AttitudeEstimatorQ::AttitudeEstimatorQ() : + _vel_prev(0, 0, 0), + _pos_acc(0, 0, 0), _voter_gyro(3), _voter_accel(3), _voter_mag(3), @@ -725,6 +727,8 @@ bool AttitudeEstimatorQ::update(float dt) corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag; } + _q.normalize(); + // Accelerometer correction // Project 'k' unit vector of earth frame to body frame // Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f)); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ab246e14a1..12f882ed99 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -846,7 +846,7 @@ Sensors::parameters_update() } else if (_parameters.battery_voltage_scaling < 0.0f) { /* apply scaling according to defaults if set to default */ -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_PX4FMU_V4) _parameters.battery_voltage_scaling = 0.0082f; #elif CONFIG_ARCH_BOARD_AEROCORE _parameters.battery_voltage_scaling = 0.0063f;