Enable UART6 and enable S.BUS. Tested to work.

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

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

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

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)

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;
 

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));

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;