Merged master into driver_framework

Images/px4fmu-v4.prototype

@@ -0,0 +1,12 @@
+{
+    "board_id": 11,
+    "magic": "PX4FWv1",
+    "description": "Firmware for the PX4FMUv4 board",
+    "image": "",
+    "build_time": 0,
+    "summary": "PX4FMUv4",
+    "version": "0.1",
+    "image_size": 0,
+    "git_identity": "",
+    "board_revision": 0
+}

Makefile

@@ -130,6 +130,9 @@ px4fmu-v1_default:
 px4fmu-v2_default:
 	$(call cmake-build,nuttx_px4fmu-v2_default)
 
+px4fmu-v4_default:
+	$(call cmake-build,nuttx_px4fmu-v4_default)
+
 px4fmu-v2_simple:
 	$(call cmake-build,nuttx_px4fmu-v2_simple)
 

ROMFS/px4fmu_common/init.d/13007_vtol_AAVVT_quad

@@ -0,0 +1,51 @@
+#!nsh
+#
+# @name Generic AAVVT v-tail plane airframe with Quad VTOL.
+#
+# @type Standard VTOL
+#
+# @maintainer Sander Smeets <sander@droneslab.com>
+#
+
+sh /etc/init.d/rc.vtol_defaults
+
+if [ $AUTOCNF == yes ]
+then
+	param set VT_TYPE 2
+	param set VT_MOT_COUNT 4
+	param set VT_TRANS_THR 0.75
+
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.15
+	param set MC_ROLLRATE_I 0.002
+	param set MC_ROLLRATE_D 0.003
+	param set MC_ROLLRATE_FF 0.0
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.12
+	param set MC_PITCHRATE_I 0.002
+	param set MC_PITCHRATE_D 0.003
+	param set MC_PITCHRATE_FF 0.0
+	param set MC_YAW_P 2.8
+	param set MC_YAW_FF 0.5
+	param set MC_YAWRATE_P 0.22
+	param set MC_YAWRATE_I 0.02
+	param set MC_YAWRATE_D 0.0
+	param set MC_YAWRATE_FF 0.0
+	
+fi
+
+set MIXER vtol_quad_x
+set PWM_OUT 12345678
+
+set MIXER_AUX vtol_AAVVT
+set PWM_AUX_RATE 50
+set PWM_AUX_OUT 1234
+set PWM_AUX_DISARMED 1000
+set PWM_AUX_MIN 1000
+set PWM_AUX_MAX 2000
+
+set MAV_TYPE 22
+
+param set VT_MOT_COUNT 4
+param set VT_IDLE_PWM_MC 1080
+param set VT_TYPE 2

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -67,32 +67,55 @@ then
 		fi
 	fi
 else
-	# FMUv1
-	if mpu6000 start
+	if ver hwcmp PX4FMU_V4
 	then
-	fi
+		# External I2C bus
+		if hmc5883 -C -T -X start
+		then
+		fi
 
-	if l3gd20 start
-	then
-	fi
+		# Internal SPI bus is rotated 90 deg yaw
+		if hmc5883 -C -T -S -R 2 start
+		then
+		fi
 
-	# MAG selection
-	if param compare SENS_EXT_MAG 2
-	then
-		if hmc5883 -C -I start
+		# Internal SPI bus ICM-20608-G is rotated 90 deg yaw
+		if mpu6000 -R 2 -T 20608 start
+		then
+		fi
+
+		# Internal SPI bus mpu9250 is rotated 90 deg yaw
+		if mpu9250 -R 2 start
 		then
 		fi
 	else
-		# Use only external as primary
-		if param compare SENS_EXT_MAG 1
+		# FMUv1
+		if mpu6000 start
 		then
-			if hmc5883 -C -X start
+		fi
+
+		if l3gd20 start
+		then
+		fi
+
+		# MAG selection
+		if param compare SENS_EXT_MAG 2
+		then
+			if hmc5883 -C -I start
 			then
 			fi
 		else
-		# auto-detect the primary, prefer external
-			if hmc5883 start
+			# Use only external as primary
+			if param compare SENS_EXT_MAG 1
 			then
+				if hmc5883 -C -X start
+				then
+				fi
+			else
+			# auto-detect the primary, prefer external
+				if hmc5883 start
+				then
+				fi
 			fi
 		fi
 	fi

ROMFS/px4fmu_common/init.d/rc.vtol_apps

@@ -18,4 +18,4 @@ fw_pos_control_l1 start
 # Start Land Detector
 # Multicopter for now until we have something for VTOL
 #
-land_detector start multicopter
+land_detector start vtol

ROMFS/px4fmu_common/init.d/rcS

@@ -217,7 +217,12 @@ then
 	#
 	if param compare SYS_USE_IO 1
 	then
-		set USE_IO yes
+		if ver hwcmp PX4FMU_V4
+		then
+			set USE_IO no
+		else
+			set USE_IO yes
+		fi
 	else
 		set USE_IO no
 	fi
@@ -537,8 +542,12 @@ then
 			# Exit from nsh to free port for mavlink
 			set EXIT_ON_END yes
 		else
-			# Start MAVLink on default port: ttyS1
 			set MAVLINK_F "-r 1200"
+			# Avoid using ttyS1 for MAVLink on FMUv4
+			if ver hwcmp PX4FMU_V4
+			then
+				set MAVLINK_F "-r 1200 -d /dev/ttyS2"
+			fi
 		fi
 	fi
 

ROMFS/px4fmu_common/mixers/quad_h.main.mix

@@ -0,0 +1,26 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a quadrotor in the H configuration.  All controls
+are mixed 100%.
+
+R: 4h 10000 10000 10000 0
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000

ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix

@@ -9,10 +9,10 @@ R: 4x 10000 10000 10000 0
 #mixer for the elevons
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 1 0  10000  10000      0 -10000  10000
-S: 1 1  10000  10000      0 -10000  10000
+S: 1 0  5000  5000      0 -10000  10000
+S: 1 1  5000  5000      0 -10000  10000
 
 M: 2
 O:       10000   10000    0 -10000  10000
-S: 1 0   10000   10000    0 -10000  10000
-S: 1 1  -10000  -10000    0 -10000  10000
+S: 1 0   5000   5000    0 -10000  10000
+S: 1 1  -5000  -5000    0 -10000  10000

ROMFS/px4fmu_common/mixers/vtol_AAVVT.aux.mix

@@ -0,0 +1,59 @@
+Aileron/v-tail/throttle VTOL mixer for PX4FMU
+=======================================================
+
+This file defines mixers suitable for controlling a fixed wing aircraft with
+aileron, v-tail (rudder, elevator) and throttle using PX4FMU.
+The configuration assumes the aileron servos are connected to PX4FMU 
+AUX servo output 0 and 1, the tail servos to output 2 and 3, the throttle
+to output 4.
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch), 2 (yaw) and 3 (thrust) 4 (flaps) 6 (flaperon).
+
+Aileron mixer (roll + flaperon)
+---------------------------------
+
+This mixer assumes that the aileron servos are set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 0  10000  10000      0 -10000  10000
+S: 1 6  10000  10000      0 -10000  10000
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 0  10000  10000      0 -10000  10000
+S: 1 6 -10000 -10000      0 -10000  10000
+
+
+V-tail mixers
+-------------
+Three scalers total (output, roll, pitch).
+
+On the assumption that the two tail servos are physically reversed, the pitch
+input is inverted between the two servos.
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 2  -7000  -7000      0 -10000  10000
+S: 1 1  -8000  -8000      0 -10000  10000
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 2  -7000  -7000      0 -10000  10000
+S: 1 1   8000   8000      0 -10000  10000
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range.  Inputs below zero are treated as zero.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 1 3      0  20000 -10000 -10000  10000
+

cmake/configs/nuttx_px4fmu-v2_default.cmake

@@ -2,6 +2,8 @@ include(nuttx/px4_impl_nuttx)
 
 set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
 
+set(config_uavcan_num_ifaces 2)
+
 set(config_module_list
 	#
 	# Board support modules

cmake/configs/nuttx_px4fmu-v4_default.cmake

@@ -0,0 +1,185 @@
+include(nuttx/px4_impl_nuttx)
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-none-eabi.cmake)
+
+set(config_uavcan_num_ifaces 1)
+
+set(config_module_list
+	#
+	# Board support modules
+	#
+	drivers/device
+	drivers/stm32
+	drivers/stm32/adc
+	drivers/stm32/tone_alarm
+	drivers/led
+	drivers/px4fmu
+	drivers/boards/px4fmu-v4
+	drivers/rgbled
+	drivers/mpu6000
+	drivers/mpu9250
+	drivers/hmc5883
+	drivers/ms5611
+	drivers/mb12xx
+	drivers/srf02
+	drivers/sf0x
+	drivers/ll40ls
+	drivers/trone
+	drivers/gps
+	drivers/pwm_out_sim
+	drivers/hott
+	drivers/hott/hott_telemetry
+	drivers/hott/hott_sensors
+	drivers/blinkm
+	drivers/airspeed
+	drivers/ets_airspeed
+	drivers/meas_airspeed
+	drivers/frsky_telemetry
+	modules/sensors
+	#drivers/mkblctrl
+	drivers/px4flow
+	drivers/oreoled
+	drivers/gimbal
+	drivers/pwm_input
+	drivers/camera_trigger
+
+	#
+	# System commands
+	#
+	systemcmds/bl_update
+	systemcmds/mixer
+	systemcmds/param
+	systemcmds/perf
+	systemcmds/pwm
+	systemcmds/esc_calib
+	systemcmds/reboot
+	#systemcmds/topic_listener
+	systemcmds/top
+	systemcmds/config
+	systemcmds/nshterm
+	systemcmds/mtd
+	systemcmds/dumpfile
+	systemcmds/ver
+	systemcmds/tests
+
+	#
+	# General system control
+	#
+	modules/commander
+	modules/navigator
+	modules/mavlink
+	modules/gpio_led
+	modules/uavcan
+	modules/land_detector
+
+	#
+	# Estimation modules (EKF/ SO3 / other filters)
+	#
+	# Too high RAM usage due to static allocations
+	# modules/attitude_estimator_ekf
+	modules/attitude_estimator_q
+	modules/ekf_att_pos_estimator
+	modules/position_estimator_inav
+
+	#
+	# Vehicle Control
+	#
+	# modules/segway # XXX Needs GCC 4.7 fix
+	modules/fw_pos_control_l1
+	modules/fw_att_control
+	modules/mc_att_control
+	modules/mc_pos_control
+	modules/vtol_att_control
+
+	#
+	# Logging
+	#
+	modules/sdlog2
+
+	#
+	# Library modules
+	#
+	modules/param
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/controllib
+	modules/uORB
+	modules/dataman
+
+	#
+	# Libraries
+	#
+	#lib/mathlib/CMSIS
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/rc
+	lib/ecl
+	lib/external_lgpl
+	lib/geo
+	lib/geo_lookup
+	lib/conversion
+	lib/launchdetection
+	lib/terrain_estimation
+	lib/runway_takeoff
+	platforms/nuttx
+
+	# had to add for cmake, not sure why wasn't in original config
+	platforms/common
+	platforms/nuttx/px4_layer
+
+	#
+	# OBC challenge
+	#
+	modules/bottle_drop
+
+	#
+	# Rover apps
+	#
+	examples/rover_steering_control
+
+	#
+	# Demo apps
+	#
+	#examples/math_demo
+	# Tutorial code from
+	# https://px4.io/dev/px4_simple_app
+	#examples/px4_simple_app
+
+	# Tutorial code from
+	# https://px4.io/dev/daemon
+	#examples/px4_daemon_app
+
+	# Tutorial code from
+	# https://px4.io/dev/debug_values
+	#examples/px4_mavlink_debug
+
+	# Tutorial code from
+	# https://px4.io/dev/example_fixedwing_control
+	#examples/fixedwing_control
+
+	# Hardware test
+	#examples/hwtest
+)
+
+set(config_extra_builtin_cmds
+	serdis
+	sercon
+	)
+
+set(config_extra_libs
+	${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
+	uavcan
+	uavcan_stm32_driver
+	)
+
+set(config_io_extra_libs
+	#${CMAKE_SOURCE_DIR}/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a
+	)
+
+add_custom_target(sercon)
+set_target_properties(sercon PROPERTIES
+	MAIN "sercon" STACK "2048")
+
+add_custom_target(serdis)
+set_target_properties(serdis PROPERTIES
+	MAIN "serdis" STACK "2048")

cmake/nuttx/px4_impl_nuttx.cmake

@@ -459,6 +459,14 @@ function(px4_os_add_flags)
 			-mfpu=fpv4-sp-d16
 			-mfloat-abi=hard
 			)
+	elseif (${BOARD} STREQUAL "px4fmu-v4")
+		set(cpu_flags
+			-mcpu=cortex-m4
+			-mthumb
+			-march=armv7e-m
+			-mfpu=fpv4-sp-d16
+			-mfloat-abi=hard
+			)
 	elseif (${BOARD} STREQUAL "aerocore")
 		set(cpu_flags
 			-mcpu=cortex-m4

msg/fw_virtual_attitude_setpoint.msg

@@ -0,0 +1,23 @@
+
+uint64 timestamp				# in microseconds since system start, is set whenever the writing thread stores new data
+
+float32 roll_body				# body angle in NED frame
+float32 pitch_body				# body angle in NED frame
+float32 yaw_body				# body angle in NED frame
+
+float32 yaw_sp_move_rate		# rad/s (commanded by user)
+
+float32[9] R_body				# Rotation matrix describing the setpoint as rotation from the current body frame
+bool R_valid					# Set to true if rotation matrix is valid
+
+# For quaternion-based attitude control
+float32[4] q_d					# Desired quaternion for quaternion control
+bool q_d_valid					# Set to true if quaternion vector is valid
+float32[4] q_e					# Attitude error in quaternion
+bool q_e_valid					# Set to true if quaternion error vector is valid
+
+float32 thrust					# Thrust in Newton the power system should generate
+
+bool roll_reset_integral			# Reset roll integral part (navigation logic change)
+bool pitch_reset_integral			# Reset pitch integral part (navigation logic change)
+bool yaw_reset_integral				# Reset yaw integral part (navigation logic change)

msg/mc_virtual_attitude_setpoint.msg

@@ -0,0 +1,23 @@
+
+uint64 timestamp				# in microseconds since system start, is set whenever the writing thread stores new data
+
+float32 roll_body				# body angle in NED frame
+float32 pitch_body				# body angle in NED frame
+float32 yaw_body				# body angle in NED frame
+
+float32 yaw_sp_move_rate		# rad/s (commanded by user)
+
+float32[9] R_body				# Rotation matrix describing the setpoint as rotation from the current body frame
+bool R_valid					# Set to true if rotation matrix is valid
+
+# For quaternion-based attitude control
+float32[4] q_d					# Desired quaternion for quaternion control
+bool q_d_valid					# Set to true if quaternion vector is valid
+float32[4] q_e					# Attitude error in quaternion
+bool q_e_valid					# Set to true if quaternion error vector is valid
+
+float32 thrust					# Thrust in Newton the power system should generate
+
+bool roll_reset_integral			# Reset roll integral part (navigation logic change)
+bool pitch_reset_integral			# Reset pitch integral part (navigation logic change)
+bool yaw_reset_integral				# Reset yaw integral part (navigation logic change)

msg/vehicle_gps_position.msg

@@ -3,6 +3,7 @@ uint64 timestamp_position	# Timestamp for position information
 int32 lat			# Latitude in 1E-7 degrees 
 int32 lon			# Longitude in 1E-7 degrees 
 int32 alt			# Altitude in 1E-3 meters (millimeters) above MSL  
+int32 alt_ellipsoid # Altitude in 1E-3 meters (millimeters) above Ellipsoid
 
 uint64 timestamp_variance
 float32 s_variance_m_s		# speed accuracy estimate m/s 

msg/vehicle_status.msg

@@ -115,6 +115,7 @@ uint32 system_id			# system id, inspired by MAVLink's system ID field
 uint32 component_id			# subsystem / component id, inspired by MAVLink's component ID field
 
 bool is_rotary_wing			# True if system is in rotary wing configuration, so for a VTOL this is only true while flying as a multicopter
+bool vtol_in_transition		# True if VTOL is doing a transition
 bool is_vtol				# True if the system is VTOL capable
 bool vtol_fw_permanent_stab		# True if vtol should stabilize attitude for fw in manual mode
 bool in_transition_mode

nuttx-configs/px4fmu-v4/include/board.h

@@ -0,0 +1,299 @@
+/************************************************************************************
+ * configs/px4fmu/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************************************/
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include <stm32.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The PX4FMUV2 uses a 24MHz crystal connected to the HSE.
+ *
+ * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c:
+ *   System Clock source           : PLL (HSE)
+ *   SYSCLK(Hz)                    : 168000000    Determined by PLL configuration
+ *   HCLK(Hz)                      : 168000000    (STM32_RCC_CFGR_HPRE)
+ *   AHB Prescaler                 : 1            (STM32_RCC_CFGR_HPRE)
+ *   APB1 Prescaler                : 4            (STM32_RCC_CFGR_PPRE1)
+ *   APB2 Prescaler                : 2            (STM32_RCC_CFGR_PPRE2)
+ *   HSE Frequency(Hz)             : 24000000     (STM32_BOARD_XTAL)
+ *   PLLM                          : 24           (STM32_PLLCFG_PLLM)
+ *   PLLN                          : 336          (STM32_PLLCFG_PLLN)
+ *   PLLP                          : 2            (STM32_PLLCFG_PLLP)
+ *   PLLQ                          : 7            (STM32_PLLCFG_PPQ)
+ *   Main regulator output voltage : Scale1 mode  Needed for high speed SYSCLK
+ *   Flash Latency(WS)             : 5
+ *   Prefetch Buffer               : OFF
+ *   Instruction cache             : ON
+ *   Data cache                    : ON
+ *   Require 48MHz for USB OTG FS, : Enabled
+ *   SDIO and RNG clock
+ */
+
+/* HSI - 16 MHz RC factory-trimmed
+ * LSI - 32 KHz RC
+ * HSE - On-board crystal frequency is 24MHz
+ * LSE - not installed
+ */
+
+#define STM32_BOARD_XTAL        24000000ul
+
+#define STM32_HSI_FREQUENCY     16000000ul
+#define STM32_LSI_FREQUENCY     32000
+#define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+//#define STM32_LSE_FREQUENCY     32768
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE
+ * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ *         = (25,000,000 / 25) * 336
+ *         = 336,000,000
+ * SYSCLK  = PLL_VCO / PLLP
+ *         = 336,000,000 / 2 = 168,000,000
+ * USB OTG FS, SDIO and RNG Clock
+ *         =  PLL_VCO / PLLQ
+ *         = 48,000,000
+ */
+
+#define STM32_PLLCFG_PLLM       RCC_PLLCFG_PLLM(24)
+#define STM32_PLLCFG_PLLN       RCC_PLLCFG_PLLN(336)
+#define STM32_PLLCFG_PLLP       RCC_PLLCFG_PLLP_2
+#define STM32_PLLCFG_PLLQ       RCC_PLLCFG_PLLQ(7)
+
+#define STM32_SYSCLK_FREQUENCY  168000000ul
+
+/* AHB clock (HCLK) is SYSCLK (168MHz) */
+
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK  /* HCLK  = SYSCLK / 1 */
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY  /* same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */
+
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLKd4     /* PCLK1 = HCLK / 4 */
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY/4)
+
+/* Timers driven from APB1 will be twice PCLK1 */
+
+#define STM32_APB1_TIM2_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+
+/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */
+
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLKd2     /* PCLK2 = HCLK / 2 */
+#define STM32_PCLK2_FREQUENCY   (STM32_HCLK_FREQUENCY/2)
+
+/* Timers driven from APB2 will be twice PCLK2 */
+
+#define STM32_APB2_TIM1_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM9_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM10_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM11_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+
+/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
+ * otherwise frequency is 2xAPBx.
+ * Note: TIM1,8 are on APB2, others on APB1
+ */
+
+#define STM32_TIM18_FREQUENCY   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_TIM27_FREQUENCY   (2*STM32_PCLK1_FREQUENCY)
+
+/* SDIO dividers.  Note that slower clocking is required when DMA is disabled
+ * in order to avoid RX overrun/TX underrun errors due to delayed responses
+ * to service FIFOs in interrupt driven mode.  These values have not been
+ * tuned!!!
+ *
+ * HCLK=72MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(178+2)=400 KHz
+ */
+
+#define SDIO_INIT_CLKDIV        (178 << SDIO_CLKCR_CLKDIV_SHIFT)
+
+/* DMA ON:  HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(2+2)=18 MHz
+ * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz
+ */
+
+#ifdef CONFIG_SDIO_DMA
+#  define SDIO_MMCXFR_CLKDIV    (2 << SDIO_CLKCR_CLKDIV_SHIFT)
+#else
+#  define SDIO_MMCXFR_CLKDIV    (3 << SDIO_CLKCR_CLKDIV_SHIFT)
+#endif
+
+/* DMA ON:  HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(1+2)=24 MHz
+ * DMA OFF: HCLK=72 MHz, SDIOCLK=72MHz, SDIO_CK=HCLK/(3+2)=14.4 MHz
+ */
+
+#ifdef CONFIG_SDIO_DMA
+#  define SDIO_SDXFR_CLKDIV     (1 << SDIO_CLKCR_CLKDIV_SHIFT)
+#else
+#  define SDIO_SDXFR_CLKDIV     (3 << SDIO_CLKCR_CLKDIV_SHIFT)
+#endif
+
+/* DMA Channl/Stream Selections *****************************************************/
+/* Stream selections are arbitrary for now but might become important in the future
+ * is we set aside more DMA channels/streams.
+ *
+ * SDIO DMA
+ *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA
+ *   DMAMAP_SDIO_2 = Channel 4, Stream 6
+ */
+
+#define DMAMAP_SDIO DMAMAP_SDIO_1
+
+/* Alternate function pin selections ************************************************/
+
+/*
+ * UARTs.
+ */
+#define GPIO_USART1_RX	GPIO_USART1_RX_2	/* ESP8266 */
+#define GPIO_USART1_TX	GPIO_USART1_TX_2
+
+#define GPIO_USART2_RX	GPIO_USART2_RX_2
+#define GPIO_USART2_TX	GPIO_USART2_TX_2
+#define GPIO_USART2_RTS	GPIO_USART2_RTS_2
+#define GPIO_USART2_CTS	GPIO_USART2_CTS_2
+
+#define GPIO_USART3_RX	GPIO_USART3_RX_3
+#define GPIO_USART3_TX	GPIO_USART3_TX_3
+#define GPIO_USART3_RTS	GPIO_USART3_RTS_2
+#define GPIO_USART3_CTS	GPIO_USART3_CTS_2
+
+#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
+
+/* UART8 has no alternate pin config */
+
+/* UART RX DMA configurations */
+#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
+
+/*
+ * CAN
+ *
+ * CAN1 is routed to the onboard transceiver.
+ */
+#define GPIO_CAN1_RX	GPIO_CAN1_RX_3
+#define GPIO_CAN1_TX	GPIO_CAN1_TX_3
+
+/*
+ * I2C
+ *
+ * The optional _GPIO configurations allow the I2C driver to manually
+ * reset the bus to clear stuck slaves.  They match the pin configuration,
+ * but are normally-high GPIOs.
+ */
+#define GPIO_I2C1_SCL		GPIO_I2C1_SCL_2
+#define GPIO_I2C1_SDA		GPIO_I2C1_SDA_2
+#define GPIO_I2C1_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8)
+#define GPIO_I2C1_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9)
+
+
+/*
+ * SPI
+ *
+ * There are sensors on SPI1, and SPI2 is connected to the FRAM.
+ */
+#define GPIO_SPI1_MISO	(GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_MOSI	(GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_SCK	(GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz)
+
+#define GPIO_SPI2_MISO	(GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI2_MOSI	(GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI2_SCK	(GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz)
+
+/************************************************************************************
+ * Public Data
+ ************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+/************************************************************************************
+ * Public Function Prototypes
+ ************************************************************************************/
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+EXTERN void stm32_boardinitialize(void);
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif  /* __ARCH_BOARD_BOARD_H */

nuttx-configs/px4fmu-v4/include/nsh_romfsimg.h

@@ -0,0 +1,42 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 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.
+ *
+ ****************************************************************************/
+
+/**
+ * nsh_romfsetc.h
+ *
+ * This file is a stub for 'make export' purposes; the actual ROMFS
+ * must be supplied by the library client.
+ */
+
+extern unsigned char romfs_img[];
+extern unsigned int romfs_img_len;

nuttx-configs/px4fmu-v4/nsh/Make.defs

@@ -0,0 +1,177 @@
+############################################################################
+# configs/px4fmu-v2/nsh/Make.defs
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+include ${TOPDIR}/.config
+include ${TOPDIR}/tools/Config.mk
+
+#
+# We only support building with the ARM bare-metal toolchain from
+# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS.
+#
+CONFIG_ARMV7M_TOOLCHAIN	:= GNU_EABI
+
+include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
+
+CC			 = $(CROSSDEV)gcc
+CXX			 = $(CROSSDEV)g++
+CPP			 = $(CROSSDEV)gcc -E
+LD			 = $(CROSSDEV)ld
+AR			 = $(CROSSDEV)ar rcs
+NM			 = $(CROSSDEV)nm
+OBJCOPY			 = $(CROSSDEV)objcopy
+OBJDUMP			 = $(CROSSDEV)objdump
+
+MAXOPTIMIZATION		 = -Os
+ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
+			   -mthumb \
+			   -march=armv7e-m \
+			   -mfpu=fpv4-sp-d16 \
+			   -mfloat-abi=hard
+
+
+# enable precise stack overflow tracking
+ifeq ($(CONFIG_ARMV7M_STACKCHECK),y)
+INSTRUMENTATIONDEFINES	 = -finstrument-functions -ffixed-r10
+endif
+
+# pull in *just* libm from the toolchain ... this is grody
+LIBM			 = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
+EXTRA_LIBS		+= $(LIBM)
+
+# use our linker script
+LDSCRIPT		 = ld.script
+
+ifeq ($(WINTOOL),y)
+  # Windows-native toolchains
+  DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+  DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+  MKDEP			 = $(TOPDIR)/tools/mknulldeps.sh
+  ARCHINCLUDES		 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
+  ARCHXXINCLUDES	 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+  ARCHSCRIPT		 = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}"
+else
+  ifeq ($(PX4_WINTOOL),y)
+    # Windows-native toolchains (MSYS)
+    DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+    DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+    MKDEP		 = $(TOPDIR)/tools/mknulldeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  else
+    # Linux/Cygwin-native toolchain
+    MKDEP		 = $(TOPDIR)/tools/mkdeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  endif
+endif
+
+# tool versions
+ARCHCCVERSION		 = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
+ARCHCCMAJOR		 = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
+
+# optimisation flags
+ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
+			   -fno-strict-aliasing \
+			   -fno-strength-reduce \
+			   -fomit-frame-pointer \
+			   -funsafe-math-optimizations \
+			   -fno-builtin-printf \
+			   -ffunction-sections \
+			   -fdata-sections
+
+ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
+ARCHOPTIMIZATION	+= -g
+endif
+
+ARCHCFLAGS		 = -std=gnu99
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHWARNINGS		 = -Wall \
+			   -Wno-sign-compare \
+			   -Wextra \
+			   -Wdouble-promotion \
+			   -Wshadow \
+			   -Wframe-larger-than=1024 \
+			   -Wpointer-arith \
+			   -Wlogical-op \
+			   -Wpacked \
+			   -Wno-unused-parameter
+#   -Wcast-qual  - generates spurious noreturn attribute warnings, try again later
+#   -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
+#   -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
+
+ARCHCWARNINGS		 = $(ARCHWARNINGS) \
+			   -Wbad-function-cast \
+			   -Wstrict-prototypes \
+			   -Wold-style-declaration \
+			   -Wmissing-parameter-type \
+			   -Wnested-externs
+ARCHWARNINGSXX		 = $(ARCHWARNINGS) \
+			   -Wno-psabi
+ARCHDEFINES		 =
+ARCHPICFLAGS		 = -fpic -msingle-pic-base -mpic-register=r10
+
+# this seems to be the only way to add linker flags
+EXTRA_LIBS		+= --warn-common \
+			   --gc-sections
+
+CFLAGS			 = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
+CPICFLAGS		 = $(ARCHPICFLAGS) $(CFLAGS)
+CXXFLAGS		 = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CXXPICFLAGS		 = $(ARCHPICFLAGS) $(CXXFLAGS)
+CPPFLAGS		 = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES)
+AFLAGS			 = $(CFLAGS) -D__ASSEMBLY__
+
+NXFLATLDFLAGS1		 = -r -d -warn-common
+NXFLATLDFLAGS2		 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
+LDNXFLATFLAGS		 = -e main -s 2048
+
+OBJEXT			 = .o
+LIBEXT			 = .a
+EXEEXT			 =
+
+
+# produce partially-linked $1 from files in $2
+define PRELINK
+	@echo "PRELINK: $1"
+	$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+endef
+
+HOSTCC			 = gcc
+HOSTINCLUDES		 = -I.
+HOSTCFLAGS		 = -Wall -Wstrict-prototypes -Wshadow -g -pipe
+HOSTLDFLAGS		 =
+

nuttx-configs/px4fmu-v4/nsh/appconfig

@@ -0,0 +1,52 @@
+############################################################################
+# configs/px4fmu/nsh/appconfig
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+# Path to example in apps/examples containing the user_start entry point
+
+CONFIGURED_APPS	+= examples/nsh
+
+# The NSH application library
+CONFIGURED_APPS += nshlib
+CONFIGURED_APPS += system/readline
+
+ifeq ($(CONFIG_CAN),y)
+#CONFIGURED_APPS += examples/can
+endif
+
+#ifeq ($(CONFIG_USBDEV),y)
+#ifeq ($(CONFIG_CDCACM),y)
+CONFIGURED_APPS += examples/cdcacm
+#endif
+#endif

nuttx-configs/px4fmu-v4/nsh/setenv.sh

@@ -0,0 +1,67 @@
+#!/bin/bash
+# configs/stm3240g-eval/nsh/setenv.sh
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+
+if [ "$_" = "$0" ] ; then
+  echo "You must source this script, not run it!" 1>&2
+  exit 1
+fi
+
+WD=`pwd`
+if [ ! -x "setenv.sh" ]; then
+  echo "This script must be executed from the top-level NuttX build directory"
+  exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then
+  export PATH_ORIG="${PATH}"
+fi
+
+# This the Cygwin path to the location where I installed the RIDE
+# toolchain under windows.  You will also have to edit this if you install
+# the RIDE toolchain in any other location
+#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin"
+
+# This the Cygwin path to the location where I installed the CodeSourcery
+# toolchain under windows.  You will also have to edit this if you install
+# the CodeSourcery toolchain in any other location
+export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin"
+
+# This the Cygwin path to the location where I build the buildroot
+# toolchain.
+#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+
+# Add the path to the toolchain to the PATH varialble
+export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"

nuttx-configs/px4fmu-v4/scripts/ld.script

@@ -0,0 +1,159 @@
+/****************************************************************************
+ * configs/px4fmu/common/ld.script
+ *
+ *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and
+ * 256Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2)  16Kb of SRAM beginning at address 0x2001:c000
+ * 3)  64Kb of SRAM beginning at address 0x2002:0000
+ * 4)  64Kb of TCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The first 0x4000 of flash is reserved for the bootloader.
+ */
+
+MEMORY
+{
+    /* disabled due to silicon errata flash (rx)   : ORIGIN = 0x08004000, LENGTH = 2032K */
+    flash (rx)   : ORIGIN = 0x08004000, LENGTH = 1008K
+    sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 192K
+    ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+}
+
+OUTPUT_ARCH(arm)
+
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/* 
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+EXTERN(_bootdelay_signature)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+                . = ALIGN(32);
+                /*
+                This signature provides the bootloader with a way to delay booting
+                */
+                _bootdelay_signature = ABSOLUTE(.);
+                FILL(0xffecc2925d7d05c5)
+                . += 8;
+		*(.text .text.*)
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)
+		*(.gnu.linkonce.t.*)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+
+		/*
+		 * This is a hack to make the newlib libm __errno() call
+		 * use the NuttX get_errno_ptr() function.
+		 */
+		__errno = get_errno_ptr;
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	/*
+	 * Construction data for parameters.
+	 */
+	__param ALIGN(4): {
+		__param_start = ABSOLUTE(.);
+		KEEP(*(__param*))
+		__param_end = ABSOLUTE(.);
+	} > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}

nuttx-configs/px4fmu-v4/src/Makefile

@@ -0,0 +1,84 @@
+############################################################################
+# configs/px4fmu/src/Makefile
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+-include $(TOPDIR)/Make.defs
+
+CFLAGS		+= -I$(TOPDIR)/sched
+
+ASRCS		=
+AOBJS		= $(ASRCS:.S=$(OBJEXT))
+
+CSRCS		= empty.c
+COBJS		= $(CSRCS:.c=$(OBJEXT))
+
+SRCS		= $(ASRCS) $(CSRCS)
+OBJS		= $(AOBJS) $(COBJS)
+
+ARCH_SRCDIR	= $(TOPDIR)/arch/$(CONFIG_ARCH)/src
+ifeq ($(WINTOOL),y)
+  CFLAGS	+= -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \
+					   -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \
+					   -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}"
+else
+  CFLAGS	+= -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m
+endif
+
+all: libboard$(LIBEXT)
+
+$(AOBJS): %$(OBJEXT): %.S
+	$(call ASSEMBLE, $<, $@)
+
+$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c
+	$(call COMPILE, $<, $@)
+
+libboard$(LIBEXT): $(OBJS)
+	$(call ARCHIVE, $@, $(OBJS))
+
+.depend: Makefile $(SRCS)
+	$(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+	$(Q) touch $@
+
+depend: .depend
+
+clean:
+	$(call DELFILE, libboard$(LIBEXT))
+	$(call CLEAN)
+
+distclean: clean
+	$(call DELFILE, Make.dep)
+	$(call DELFILE, .depend)
+
+-include Make.dep
+

nuttx-configs/px4fmu-v4/src/empty.c

@@ -0,0 +1,4 @@
+/*
+ * There are no source files here, but libboard.a can't be empty, so
+ * we have this empty source file to keep it company.
+ */

src/drivers/boards/px4fmu-v1/px4fmu_init.c

@@ -145,20 +145,6 @@ static struct spi_dev_s *spi3;
 
 #include <math.h>
 
-#if 0
-#ifdef __cplusplus
-__EXPORT int matherr(struct __exception *e)
-{
-	return 1;
-}
-#else
-__EXPORT int matherr(struct exception *e)
-{
-	return 1;
-}
-#endif
-#endif
-
 __EXPORT int nsh_archinitialize(void)
 {
 	int result;

src/drivers/boards/px4fmu-v2/CMakeLists.txt

@@ -44,4 +44,4 @@ px4_add_module(
 	DEPENDS
 		platforms__common
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/drivers/boards/px4fmu-v2/px4fmu2_init.c

@@ -212,19 +212,6 @@ static struct sdio_dev_s *sdio;
 
 #include <math.h>
 
-/* TODO XXX commented this out to get cmake build working */
-/*#ifdef __cplusplus*/
-/*__EXPORT int matherr(struct __exception *e)*/
-/*{*/
-/*return 1;*/
-/*}*/
-/*#else*/
-/*__EXPORT int matherr(struct exception *e)*/
-/*{*/
-/*return 1;*/
-/*}*/
-/*#endif*/
-
 __EXPORT int nsh_archinitialize(void)
 {
 

src/drivers/boards/px4fmu-v4/CMakeLists.txt

@@ -0,0 +1,47 @@
+############################################################################
+#
+#   Copyright (c) 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
+# 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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__boards__px4fmu-v4
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		px4fmu_can.c
+		px4fmu_init.c
+		px4fmu_pwm_servo.c
+		px4fmu_spi.c
+		px4fmu_usb.c
+		px4fmu_led.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/drivers/boards/px4fmu-v4/board_config.h

@@ -0,0 +1,282 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 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 board_config.h
+ *
+ * PX4FMUv2 internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <px4_config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+
+#define UDID_START		0x1FFF7A10
+
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+/* Configuration ************************************************************************************/
+//{GPIO_RSSI_IN,           0,                       0}, - pio Analog used as PWM
+//{0,                      GPIO_LED_SAFETY,         0},	pio replacement
+//{GPIO_SAFETY_SWITCH_IN,  0,                       0},   pio replacement
+//{0,                      GPIO_PERIPH_3V3_EN,      0},	Owned by the 8266 driver
+//{0,                      GPIO_SBUS_INV,           0},	https://github.com/PX4/Firmware/blob/master/src/modules/px4iofirmware/sbus.c
+//{GPIO_8266_GPIO0,        0,                       0},   Owned by the 8266 driver
+//{0,                      GPIO_SPEKTRUM_POWER,     0},	Owned Spektum driver input to auto pilot
+//{0,                      GPIO_8266_PD,            0},	Owned by the 8266 driver
+//{0,                      GPIO_8266_RST,           0},	Owned by the 8266 driver
+
+/* PX4FMU GPIOs ***********************************************************************************/
+/* LEDs */
+
+#define GPIO_LED1		(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN11)
+#define GPIO_LED2		(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_LED3		(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN3)
+
+#define GPIO_LED_RED 	GPIO_LED1
+#define GPIO_LED_GREEN 	GPIO_LED2
+#define GPIO_LED_BLUE   GPIO_LED3
+
+/*  Define the Chip Selects */
+
+#define GPIO_SPI_CS_MPU9250		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
+#define GPIO_SPI_CS_HMC5983		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15)
+#define GPIO_SPI_CS_MS5611		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
+#define GPIO_SPI_CS_ICM_20608_G (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
+
+#define GPIO_SPI_CS_FRAM		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+
+/*  Define the Ready interrupts */
+
+#define GPIO_DRDY_MPU9250		(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15)
+#define GPIO_DRDY_HMC5983		(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTE|GPIO_PIN12)
+#define GPIO_DRDY_ICM_20608_G	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN14)
+
+
+/*
+ *  Define the ability to shut off off the sensor signals
+ *  by changing the signals to inputs
+ */
+
+#define _PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz))
+
+#define GPIO_SPI_CS_OFF_MPU9250		_PIN_OFF(GPIO_SPI_CS_MPU9250)
+#define GPIO_SPI_CS_OFF_HMC5983		_PIN_OFF(GPIO_SPI_CS_HMC5983)
+#define GPIO_SPI_CS_OFF_MS5611		_PIN_OFF(GPIO_SPI_CS_MS5611)
+#define GPIO_SPI_CS_OFF_ICM_20608_G _PIN_OFF(GPIO_SPI_CS_ICM_20608_G)
+
+#define GPIO_DRDY_OFF_MPU9250		_PIN_OFF(GPIO_DRDY_MPU9250)
+#define GPIO_DRDY_OFF_HMC5983		_PIN_OFF(GPIO_DRDY_HMC5983)
+#define GPIO_DRDY_OFF_ICM_20608_G	_PIN_OFF(GPIO_DRDY_ICM_20608_G)
+
+
+/* SPI1 off */
+#define GPIO_SPI1_SCK_OFF	_PIN_OFF(GPIO_SPI1_SCK)
+#define GPIO_SPI1_MISO_OFF	_PIN_OFF(GPIO_SPI1_MISO)
+#define GPIO_SPI1_MOSI_OFF	_PIN_OFF(GPIO_SPI1_MOSI)
+
+#define PX4_SPI_BUS_SENSORS	1
+#define PX4_SPI_BUS_RAMTRON	2
+
+/* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
+#define PX4_SPIDEV_GYRO			1
+#define PX4_SPIDEV_ACCEL_MAG	2
+#define PX4_SPIDEV_BARO			3
+#define PX4_SPIDEV_MPU			4
+#define PX4_SPIDEV_HMC			5
+#define PX4_SPIDEV_ICM			6
+
+/* I2C busses */
+#define PX4_I2C_BUS_EXPANSION	1
+#define PX4_I2C_BUS_LED			PX4_I2C_BUS_EXPANSION
+
+/* Devices on the external bus.
+ *
+ * Note that these are unshifted addresses.
+ */
+#define PX4_I2C_OBDEV_LED	0x55
+#define PX4_I2C_OBDEV_HMC5883	0x1e
+
+/*
+ * ADC channels
+ *
+ * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver
+ */
+#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15)
+
+// ADC defines to be used in sensors.cpp to read from a particular channel
+#define ADC_BATTERY_VOLTAGE_CHANNEL		2
+#define ADC_BATTERY_CURRENT_CHANNEL		3
+#define ADC_5V_RAIL_SENSE				4
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL	15
+
+/* User GPIOs
+ *
+ * GPIO0-5 are the PWM servo outputs.
+ */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
+
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13)
+#define GPIO_GPIO2_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN14)
+
+/* Power supply control and monitoring GPIOs */
+#define GPIO_VDD_BRICK_VALID	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_VDD_3V3_SENSORS_EN	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+
+/* Tone alarm output */
+#define TONE_ALARM_TIMER		2	/* timer 2 */
+#define TONE_ALARM_CHANNEL		1	/* channel 1 */
+#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15)
+#define GPIO_TONE_ALARM			(GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN15)
+
+/* PWM
+ *
+ * Six PWM outputs are configured.
+ *
+ * Pins:
+ *
+ * CH1 : PE14 : TIM1_CH4
+ * CH2 : PE13 : TIM1_CH3
+ * CH3 : PE11 : TIM1_CH2
+ * CH4 : PE9  : TIM1_CH1
+ * CH5 : PD13 : TIM4_CH2
+ * CH6 : PD14 : TIM4_CH3
+ */
+#define GPIO_TIM1_CH1OUT	GPIO_TIM1_CH1OUT_2
+#define GPIO_TIM1_CH2OUT	GPIO_TIM1_CH2OUT_2
+#define GPIO_TIM1_CH3OUT	GPIO_TIM1_CH3OUT_2
+#define GPIO_TIM1_CH4OUT	GPIO_TIM1_CH4OUT_2
+#define GPIO_TIM4_CH2OUT	GPIO_TIM4_CH2OUT_2
+#define GPIO_TIM4_CH3OUT	GPIO_TIM4_CH3OUT_2
+
+/* USB OTG FS
+ *
+ * PA9  OTG_FS_VBUS VBUS sensing
+ */
+#define GPIO_OTGFS_VBUS		(GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9)
+
+/* High-resolution timer */
+#define HRT_TIMER		3	/* use timer8 for the HRT */
+#define HRT_TIMER_CHANNEL	4	/* use capture/compare channel */
+
+#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"
+
+/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */
+#define PWMIN_TIMER			4
+#define PWMIN_TIMER_CHANNEL	2
+#define GPIO_PWM_IN			GPIO_TIM4_CH2IN_2
+
+#define GPIO_RSSI_IN 			(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
+#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_SET|GPIO_PORTC|GPIO_PIN5)
+#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)
+#define GPIO_8266_PD			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5)
+#define GPIO_8266_RST			(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6)
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+extern void stm32_usbinitialize(void);
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization for NSH.
+ *
+ *   CONFIG_NSH_ARCHINIT=y :
+ *     Called from the NSH library
+ *
+ *   CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
+ *   CONFIG_NSH_ARCHINIT=n :
+ *     Called from board_initialize().
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_NSH_LIBRARY
+int nsh_archinitialize(void);
+#endif
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS

src/drivers/boards/px4fmu-v4/px4fmu_can.c

@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 px4fmu_can.c
+ *
+ * Board-specific CAN functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_config.h>
+
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/can.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "up_arch.h"
+
+#include "stm32.h"
+#include "stm32_can.h"
+#include "board_config.h"
+
+#ifdef CONFIG_CAN
+
+/************************************************************************************
+ * Pre-processor Definitions
+ ************************************************************************************/
+/* Configuration ********************************************************************/
+
+#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
+#  warning "Both CAN1 and CAN2 are enabled.  Assuming only CAN1."
+#  undef CONFIG_STM32_CAN2
+#endif
+
+#ifdef CONFIG_STM32_CAN1
+#  define CAN_PORT 1
+#else
+#  define CAN_PORT 2
+#endif
+
+/* Debug ***************************************************************************/
+/* Non-standard debug that may be enabled just for testing CAN */
+
+#ifdef CONFIG_DEBUG_CAN
+#  define candbg    dbg
+#  define canvdbg   vdbg
+#  define canlldbg  lldbg
+#  define canllvdbg llvdbg
+#else
+#  define candbg(x...)
+#  define canvdbg(x...)
+#  define canlldbg(x...)
+#  define canllvdbg(x...)
+#endif
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: can_devinit
+ *
+ * Description:
+ *   All STM32 architectures must provide the following interface to work with
+ *   examples/can.
+ *
+ ************************************************************************************/
+
+int can_devinit(void)
+{
+	static bool initialized = false;
+	struct can_dev_s *can;
+	int ret;
+
+	/* Check if we have already initialized */
+
+	if (!initialized) {
+		/* Call stm32_caninitialize() to get an instance of the CAN interface */
+
+		can = stm32_caninitialize(CAN_PORT);
+
+		if (can == NULL) {
+			candbg("ERROR:  Failed to get CAN interface\n");
+			return -ENODEV;
+		}
+
+		/* Register the CAN driver at "/dev/can0" */
+
+		ret = can_register("/dev/can0", can);
+
+		if (ret < 0) {
+			candbg("ERROR: can_register failed: %d\n", ret);
+			return ret;
+		}
+
+		/* Now we are initialized */
+
+		initialized = true;
+	}
+
+	return OK;
+}
+
+#endif

src/drivers/boards/px4fmu-v4/px4fmu_init.c

@@ -0,0 +1,335 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 px4fmu_init.c
+ *
+ * PX4FMU-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <px4_config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/sdio.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/gran.h>
+
+#include <stm32.h>
+#include "board_config.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+#include <systemlib/perf_counter.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+__END_DECLS
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+#if defined(CONFIG_FAT_DMAMEMORY)
+# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
+#  error microSD DMA support requires CONFIG_GRAN
+# endif
+
+static GRAN_HANDLE dma_allocator;
+
+/*
+ * The DMA heap size constrains the total number of things that can be
+ * ready to do DMA at a time.
+ *
+ * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
+ * one sector-sized buffer per file.
+ *
+ * We use a fundamental alignment / granule size of 64B; this is sufficient
+ * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
+ */
+static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
+static perf_counter_t g_dma_perf;
+
+static void
+dma_alloc_init(void)
+{
+	dma_allocator = gran_initialize(g_dma_heap,
+					sizeof(g_dma_heap),
+					7,  /* 128B granule - must be > alignment (XXX bug?) */
+					6); /* 64B alignment */
+
+	if (dma_allocator == NULL) {
+		message("[boot] DMA allocator setup FAILED");
+
+	} else {
+		g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+	}
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/*
+ * DMA-aware allocator stubs for the FAT filesystem.
+ */
+
+__EXPORT void *fat_dma_alloc(size_t size);
+__EXPORT void fat_dma_free(FAR void *memory, size_t size);
+
+void *
+fat_dma_alloc(size_t size)
+{
+	perf_count(g_dma_perf);
+	return gran_alloc(dma_allocator, size);
+}
+
+void
+fat_dma_free(FAR void *memory, size_t size)
+{
+	gran_free(dma_allocator, memory, size);
+}
+
+#else
+
+# define dma_alloc_init()
+
+#endif
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void
+stm32_boardinitialize(void)
+{
+	/* configure SPI interfaces */
+	stm32_spiinitialize();
+
+	/* configure LEDs */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi1;
+static struct spi_dev_s *spi2;
+static struct sdio_dev_s *sdio;
+
+#include <math.h>
+
+__EXPORT int nsh_archinitialize(void)
+{
+
+	/* configure ADC pins */
+	stm32_configgpio(GPIO_ADC1_IN2);	/* BATT_VOLTAGE_SENS */
+	stm32_configgpio(GPIO_ADC1_IN3);	/* BATT_CURRENT_SENS */
+	stm32_configgpio(GPIO_ADC1_IN4);	/* VDD_5V_SENS */
+
+	stm32_configgpio(GPIO_ADC1_IN13);	/* FMU_AUX_ADC_1 */
+	stm32_configgpio(GPIO_ADC1_IN14);	/* FMU_AUX_ADC_2 */
+	stm32_configgpio(GPIO_ADC1_IN15);	/* PRESSURE_SENS */
+
+	/* configure power supply control/sense pins */
+	stm32_configgpio(GPIO_PERIPH_3V3_EN);
+	stm32_configgpio(GPIO_VDD_BRICK_VALID);
+	stm32_configgpio(GPIO_GPIO5_OUTPUT);
+
+	stm32_configgpio(GPIO_SBUS_INV);
+	stm32_configgpio(GPIO_8266_GPIO0);
+	stm32_configgpio(GPIO_SPEKTRUM_POWER);
+	stm32_configgpio(GPIO_8266_PD);
+	stm32_configgpio(GPIO_8266_RST);
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/* configure the DMA allocator */
+	dma_alloc_init();
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)stm32_serial_dma_poll,
+		       NULL);
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_RED);
+	led_off(LED_GREEN);
+	led_off(LED_BLUE);
+
+	/* Configure SPI-based devices */
+
+	spi1 = up_spiinitialize(1);
+
+	if (!spi1) {
+		message("[boot] FAILED to initialize SPI port 1\n");
+		up_ledon(LED_RED);
+		return -ENODEV;
+	}
+
+	/* Default SPI1 to 1MHz and de-assert the known chip selects. */
+	SPI_SETFREQUENCY(spi1, 10000000);
+	SPI_SETBITS(spi1, 8);
+	SPI_SETMODE(spi1, SPIDEV_MODE3);
+	SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_HMC, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
+	SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
+	up_udelay(20);
+
+	/* Get the SPI port for the FRAM */
+
+	spi2 = up_spiinitialize(2);
+
+	if (!spi2) {
+		message("[boot] FAILED to initialize SPI port 2\n");
+		up_ledon(LED_RED);
+		return -ENODEV;
+	}
+
+	/* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max)
+	 * and de-assert the known chip selects. */
+
+	// XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated
+	SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000);
+	SPI_SETBITS(spi2, 8);
+	SPI_SETMODE(spi2, SPIDEV_MODE3);
+	SPI_SELECT(spi2, SPIDEV_FLASH, false);
+
+#ifdef CONFIG_MMCSD
+	/* First, get an instance of the SDIO interface */
+
+	sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
+
+	if (!sdio) {
+		message("[boot] Failed to initialize SDIO slot %d\n",
+			CONFIG_NSH_MMCSDSLOTNO);
+		return -ENODEV;
+	}
+
+	/* Now bind the SDIO interface to the MMC/SD driver */
+	int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
+
+	if (ret != OK) {
+		message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
+		return ret;
+	}
+
+	/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
+	sdio_mediachange(sdio, true);
+
+#endif
+
+	return OK;
+}

src/drivers/boards/px4fmu-v4/px4fmu_led.c

@@ -0,0 +1,106 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 px4fmu2_led.c
+ *
+ * PX4FMU LED backend.
+ */
+
+#include <px4_config.h>
+
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
+
+/*
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
+ */
+__BEGIN_DECLS
+extern void led_init(void);
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
+
+
+
+static uint32_t g_ledmap[] = {
+	GPIO_LED_BLUE,    // Indexed by LED_BLUE
+	GPIO_LED_RED,     // Indexed by LED_RED, LED_AMBER
+	GPIO_LED_SAFETY,  // Indexed by LED_SAFETY
+	GPIO_LED_GREEN,   // Indexed by LED_GREEN
+};
+
+__EXPORT void led_init(void)
+{
+	/* Configure LED GPIOs for output */
+	for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) {
+		stm32_configgpio(g_ledmap[l]);
+	}
+}
+
+static void phy_set_led(int led, bool state)
+{
+	/* Pull Down to switch on */
+	stm32_gpiowrite(g_ledmap[led], !state);
+}
+
+static bool phy_get_led(int led)
+{
+
+	return !stm32_gpioread(g_ledmap[led]);
+}
+
+__EXPORT void led_on(int led)
+{
+	phy_set_led(led, true);
+}
+
+__EXPORT void led_off(int led)
+{
+	phy_set_led(led, false);
+}
+
+__EXPORT void led_toggle(int led)
+{
+
+	phy_set_led(led, !phy_get_led(led));
+}

src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 px4fmu_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM1_BASE,
+		.clock_register = STM32_RCC_APB2ENR,
+		.clock_bit = RCC_APB2ENR_TIM1EN,
+		.clock_freq = STM32_APB2_TIM1_CLKIN
+	},
+	{
+		.base = STM32_TIM4_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM4EN,
+		.clock_freq = STM32_APB1_TIM4_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM1_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM1_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1000,
+	},
+	{
+		.gpio = GPIO_TIM4_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1000,
+	}
+};

src/drivers/boards/px4fmu-v4/px4fmu_spi.c

@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 px4fmu_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include <up_arch.h>
+#include <chip.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_spiinitialize(void)
+{
+#ifdef CONFIG_STM32_SPI1
+	stm32_configgpio(GPIO_SPI_CS_MPU9250);
+	stm32_configgpio(GPIO_SPI_CS_HMC5983);
+	stm32_configgpio(GPIO_SPI_CS_MS5611);
+	stm32_configgpio(GPIO_SPI_CS_ICM_20608_G);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
+
+	stm32_configgpio(GPIO_DRDY_MPU9250);
+	stm32_configgpio(GPIO_DRDY_HMC5983);
+	stm32_configgpio(GPIO_DRDY_ICM_20608_G);
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+	stm32_configgpio(GPIO_SPI_CS_FRAM);
+	stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1);
+#endif
+
+}
+
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* SPI select is active low, so write !selected to select the device */
+
+	switch (devid) {
+	case PX4_SPIDEV_ICM:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, !selected);
+		break;
+
+	case PX4_SPIDEV_ACCEL_MAG:
+		/* Making sure the other peripherals are not selected */
+		break;
+
+	case PX4_SPIDEV_BARO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MS5611, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
+		break;
+
+	case PX4_SPIDEV_HMC:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_HMC5983, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
+		break;
+
+	case PX4_SPIDEV_MPU:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_MPU9250, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MS5611, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
+		break;
+
+	default:
+		break;
+	}
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+
+
+#ifdef CONFIG_STM32_SPI2
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there can only be one device on this bus, so always select it */
+	stm32_gpiowrite(GPIO_SPI_CS_FRAM, !selected);
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	/* FRAM is always present */
+	return SPI_STATUS_PRESENT;
+}
+#endif

src/drivers/boards/px4fmu-v4/px4fmu_usb.c

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 px4fmu_usb.c
+ *
+ * Board-specific USB functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/usb/usbdev.h>
+#include <nuttx/usb/usbdev_trace.h>
+
+#include <up_arch.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_usbinitialize
+ *
+ * Description:
+ *   Called to setup USB-related GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbinitialize(void)
+{
+	/* The OTG FS has an internal soft pull-up */
+
+	/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
+
+#ifdef CONFIG_STM32_OTGFS
+	stm32_configgpio(GPIO_OTGFS_VBUS);
+	/* XXX We only support device mode
+	stm32_configgpio(GPIO_OTGFS_PWRON);
+	stm32_configgpio(GPIO_OTGFS_OVER);
+	*/
+#endif
+}
+
+/************************************************************************************
+ * Name:  stm32_usbsuspend
+ *
+ * Description:
+ *   Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is
+ *   used.  This function is called whenever the USB enters or leaves suspend mode.
+ *   This is an opportunity for the board logic to shutdown clocks, power, etc.
+ *   while the USB is suspended.
+ *
+ ************************************************************************************/
+
+__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume)
+{
+	ulldbg("resume: %d\n", resume);
+}
+

src/drivers/boards/px4io-v1/board_config.h

@@ -65,6 +65,8 @@
 #define GPIO_LED3       (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
 			 GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
 
+#define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10)
+
 /* Safety switch button *************************************************************/
 
 #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)

src/drivers/boards/px4io-v2/board_config.h

@@ -79,6 +79,8 @@
 #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
 #define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
 
+#define GPIO_USART1_RX_SPEKTRUM		(GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10)
+
 /* Safety switch button *******************************************************/
 
 #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)

src/drivers/drv_gpio.h

@@ -94,6 +94,29 @@
 
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4
+/*
+ * PX4FMUv3 GPIO numbers.
+ *
+ * There are no alternate functions on this board.
+ */
+# define GPIO_SERVO_1			(1<<0)		/**< servo 1 output */
+# define GPIO_SERVO_2			(1<<1)		/**< servo 2 output */
+# define GPIO_SERVO_3			(1<<2)		/**< servo 3 output */
+# define GPIO_SERVO_4			(1<<3)		/**< servo 4 output */
+# define GPIO_SERVO_5			(1<<4)		/**< servo 5 output */
+# define GPIO_SERVO_6			(1<<5)		/**< servo 6 output */
+
+# define GPIO_3V3_SENSORS_EN	(1<<7)		/**< PE3 - VDD_3V3_SENSORS_EN */
+# define GPIO_BRICK_VALID		(1<<8)		/**< PB5 - !VDD_BRICK_VALID */
+
+/**
+ * Device paths for things that support the GPIO ioctl protocol.
+ */
+# define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
+
+#endif
+
 #ifdef CONFIG_ARCH_BOARD_AEROCORE
 /*
  * AeroCore GPIO numbers and configuration.
@@ -121,7 +144,7 @@
 #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)  && \
 	!defined(CONFIG_ARCH_BOARD_PX4FMU_V1) && !defined(CONFIG_ARCH_BOARD_PX4FMU_V2) && \
 	!defined(CONFIG_ARCH_BOARD_AEROCORE) && !defined(CONFIG_ARCH_BOARD_PX4_STM32F4DISCOVERY) && \
-	!defined(CONFIG_ARCH_BOARD_SITL)
+	!defined(CONFIG_ARCH_BOARD_PX4FMU_V4) && !defined(CONFIG_ARCH_BOARD_SITL)
 # error No CONFIG_ARCH_BOARD_xxxx set
 #endif
 /*

src/drivers/drv_led.h

@@ -53,6 +53,8 @@
 #define LED_RED			1	/* some boards have red rather than amber */
 #define LED_BLUE		0
 #define LED_SAFETY		2
+#define LED_GREEN		3
+
 
 #define LED_ON			_PX4_IOC(_LED_BASE, 0)
 #define LED_OFF			_PX4_IOC(_LED_BASE, 1)

src/drivers/gps/ubx.cpp

@@ -874,6 +874,7 @@ UBX::payload_rx_done(void)
 		_gps_position->alt	= _buf.payload_rx_nav_posllh.hMSL;
 		_gps_position->eph	= (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m
 		_gps_position->epv	= (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m
+		_gps_position->alt_ellipsoid = _buf.payload_rx_nav_posllh.height;
 
 		_gps_position->timestamp_position = hrt_absolute_time();
 

src/drivers/ll40ls/ll40ls.cpp

@@ -97,7 +97,7 @@ LidarLite *get_dev(const bool use_i2c, const int bus)
 	LidarLite *g_dev = nullptr;
 
 	if (use_i2c) {
-		g_dev = static_cast<LidarLite *>(bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
+		g_dev = static_cast<LidarLite *>(bus == PX4_I2C_BUS_EXPANSION ? g_dev_ext : g_dev_int);
 
 		if (g_dev == nullptr) {
 			errx(1, "i2c driver not running");
@@ -231,11 +231,15 @@ void start(const bool use_i2c, const int bus)
 
 fail:
 
+#ifdef PX4_I2C_BUS_ONBOARD
+
 	if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
 		delete g_dev_int;
 		g_dev_int = nullptr;
 	}
 
+#endif
+
 	if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) {
 		delete g_dev_ext;
 		g_dev_ext = nullptr;
@@ -255,17 +259,17 @@ fail:
 void stop(const bool use_i2c, const int bus)
 {
 	if (use_i2c) {
-		if (bus == PX4_I2C_BUS_ONBOARD) {
-			if (g_dev_int != nullptr) {
-				delete g_dev_int;
-				g_dev_int = nullptr;
-			}
-
-		} else {
+		if (bus == PX4_I2C_BUS_EXPANSION) {
 			if (g_dev_ext != nullptr) {
 				delete g_dev_ext;
 				g_dev_ext = nullptr;
 			}
+
+		} else {
+			if (g_dev_int != nullptr) {
+				delete g_dev_int;
+				g_dev_int = nullptr;
+			}
 		}
 
 	} else {
@@ -293,7 +297,7 @@ test(const bool use_i2c, const int bus)
 	const char *path;
 
 	if (use_i2c) {
-		path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT);
+		path = ((bus == PX4_I2C_BUS_EXPANSION) ? LL40LS_DEVICE_PATH_EXT : LL40LS_DEVICE_PATH_INT);
 
 	} else {
 		path = LL40LS_DEVICE_PATH_PWM;
@@ -366,7 +370,7 @@ reset(const bool use_i2c, const int bus)
 	const char *path;
 
 	if (use_i2c) {
-		path = ((bus == PX4_I2C_BUS_ONBOARD) ? LL40LS_DEVICE_PATH_INT : LL40LS_DEVICE_PATH_EXT);
+		path = ((bus == PX4_I2C_BUS_EXPANSION) ? LL40LS_DEVICE_PATH_EXT : LL40LS_DEVICE_PATH_INT);
 
 	} else {
 		path = LL40LS_DEVICE_PATH_PWM;

src/drivers/meas_airspeed/meas_airspeed.cpp

@@ -335,7 +335,7 @@ MEASAirspeed::cycle()
 void
 MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
 {
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
 
 	if (_t_system_power == -1) {
 		_t_system_power = orb_subscribe(ORB_ID(system_power));
@@ -389,7 +389,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
 	}
 
 	temperature -= voltage_diff * temp_slope;
-#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
+#endif // defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
 }
 
 /**

src/drivers/mpu6000/mpu6000.cpp

@@ -149,6 +149,21 @@
 #define BIT_INT_STATUS_DATA		0x01
 
 #define MPU_WHOAMI_6000			0x68
+#define ICM_WHOAMI_20608		0xaf
+
+// ICM2608 specific registers
+
+/* this is an undocumented register which
+   if set incorrectly results in getting a 2.7m/s/s offset
+   on the Y axis of the accelerometer
+*/
+#define MPUREG_ICM_UNDOC1		0x11
+#define MPUREG_ICM_UNDOC1_VALUE		0xc9
+
+// Product ID Description for ICM2608
+// There is none
+
+#define ICM20608_REV_00		0
 
 // Product ID Description for MPU6000
 // high 4 bits 	low 4 bits
@@ -208,7 +223,8 @@ class MPU6000_gyro;
 class MPU6000 : public device::SPI
 {
 public:
-	MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation);
+	MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation,
+		int device_type);
 	virtual ~MPU6000();
 
 	virtual int		init();
@@ -242,6 +258,7 @@ protected:
 	virtual int		gyro_ioctl(struct file *filp, int cmd, unsigned long arg);
 
 private:
+	int 			_device_type;
 	MPU6000_gyro		*_gyro;
 	uint8_t			_product;	/** product code */
 
@@ -292,7 +309,7 @@ private:
 	// this is used to support runtime checking of key
 	// configuration registers to detect SPI bus errors and sensor
 	// reset
-#define MPU6000_NUM_CHECKED_REGISTERS 9
+#define MPU6000_NUM_CHECKED_REGISTERS 10
 	static const uint8_t	_checked_registers[MPU6000_NUM_CHECKED_REGISTERS];
 	uint8_t			_checked_values[MPU6000_NUM_CHECKED_REGISTERS];
 	uint8_t			_checked_next;
@@ -325,6 +342,15 @@ private:
 	 */
 	int			reset();
 
+	/**
+	 * is_icm_device
+	 */
+	bool 		is_icm_device() { return _device_type == 20608;}
+	/**
+	 * is_mpu_device
+	 */
+	bool 		is_mpu_device() { return _device_type == 6000;}
+
 	/**
 	 * Static trampoline from the hrt_call context; because we don't have a
 	 * generic hrt wrapper yet.
@@ -468,7 +494,8 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU
 									     MPUREG_GYRO_CONFIG,
 									     MPUREG_ACCEL_CONFIG,
 									     MPUREG_INT_ENABLE,
-									     MPUREG_INT_PIN_CFG
+									     MPUREG_INT_PIN_CFG,
+									     MPUREG_ICM_UNDOC1
 									   };
 
 
@@ -506,8 +533,10 @@ private:
 /** driver 'main' command */
 extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
 
-MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) :
+MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation,
+		 int device_type) :
 	SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
+	_device_type(device_type),
 	_gyro(new MPU6000_gyro(this, path_gyro)),
 	_product(0),
 	_call{},
@@ -768,6 +797,10 @@ int MPU6000::reset()
 	write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
 	usleep(1000);
 
+	if (is_icm_device()) {
+		write_checked_reg(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE);
+	}
+
 	// Oscillator set
 	// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
 	usleep(1000);
@@ -778,16 +811,15 @@ int MPU6000::reset()
 int
 MPU6000::probe()
 {
-	uint8_t whoami;
-	whoami = read_reg(MPUREG_WHOAMI);
+	uint8_t whoami = read_reg(MPUREG_WHOAMI);
+	uint8_t expected = is_mpu_device() ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
 
-	if (whoami != MPU_WHOAMI_6000) {
+	if (whoami != expected) {
 		DEVICE_DEBUG("unexpected WHOAMI 0x%02x", whoami);
 		return -EIO;
-
 	}
 
-	/* look for a product ID we recognise */
+	/* look for a product ID we recognize */
 	_product = read_reg(MPUREG_PRODUCT_ID);
 
 	// verify product revision
@@ -804,6 +836,7 @@ MPU6000::probe()
 	case MPU6000_REV_D8:
 	case MPU6000_REV_D9:
 	case MPU6000_REV_D10:
+	case ICM20608_REV_00:
 		DEVICE_DEBUG("ID 0x%02x", _product);
 		_checked_values[0] = _product;
 		return OK;
@@ -1483,6 +1516,13 @@ MPU6000::read_reg(unsigned reg, uint32_t speed)
 {
 	uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
 
+	// There is no MPUREG_PRODUCT_ID on the icm device
+	// so lets make dummy it up and allow the rest of the
+	// code to run as is
+	if (reg == MPUREG_PRODUCT_ID && is_icm_device()) {
+		return ICM20608_REV_00;
+	}
+
 	// general register transfer at low clock speed
 	set_frequency(speed);
 
@@ -1641,6 +1681,11 @@ MPU6000::check_registers(void)
 	*/
 	uint8_t v;
 
+	// the MPUREG_ICM_UNDOC1 is specific to the ICM20608 (and undocumented)
+	if (_checked_registers[_checked_next] == MPUREG_ICM_UNDOC1 && !is_icm_device()) {
+		_checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS;
+	}
+
 	if ((v = read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) !=
 	    _checked_values[_checked_next]) {
 		/*
@@ -2051,7 +2096,7 @@ namespace mpu6000
 MPU6000	*g_dev_int; // on internal bus
 MPU6000	*g_dev_ext; // on external bus
 
-void	start(bool, enum Rotation, int range);
+void	start(bool, enum Rotation, int range, int device_type);
 void	stop(bool);
 void	test(bool);
 void	reset(bool);
@@ -2068,7 +2113,7 @@ void	usage();
  * or failed to detect the sensor.
  */
 void
-start(bool external_bus, enum Rotation rotation, int range)
+start(bool external_bus, enum Rotation rotation, int range, int device_type)
 {
 	int fd;
 	MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
@@ -2084,13 +2129,23 @@ start(bool external_bus, enum Rotation rotation, int range)
 	/* create the driver */
 	if (external_bus) {
 #ifdef PX4_SPI_BUS_EXT
-		*g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation);
+# if defined(PX4_SPIDEV_EXT_ICM)
+		spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_EXT_MPU : PX4_SPIDEV_EXT_ICM);
+# else
+		spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_EXT_MPU;
+# endif
+		*g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, cs, rotation, device_type);
 #else
 		errx(0, "External SPI not available");
 #endif
 
 	} else {
-		*g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
+#if defined(PX4_SPIDEV_ICM)
+		spi_dev_e cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_MPU : PX4_SPIDEV_ICM);
+#else
+		spi_dev_e cs = (spi_dev_e) PX4_SPIDEV_MPU;
+#endif
+		*g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, cs, rotation, device_type);
 	}
 
 	if (*g_dev_ptr == nullptr) {
@@ -2334,6 +2389,7 @@ usage()
 	warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'factorytest', 'testerror'");
 	warnx("options:");
 	warnx("    -X    (external bus)");
+	warnx("    -M 6000|20608 (default 6000)");
 	warnx("    -R rotation");
 	warnx("    -a accel range (in g)");
 }
@@ -2344,17 +2400,22 @@ int
 mpu6000_main(int argc, char *argv[])
 {
 	bool external_bus = false;
+	int device_type = 6000;
 	int ch;
 	enum Rotation rotation = ROTATION_NONE;
 	int accel_range = 8;
 
 	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc, argv, "XR:a:")) != EOF) {
+	while ((ch = getopt(argc, argv, "T:XR:a:")) != EOF) {
 		switch (ch) {
 		case 'X':
 			external_bus = true;
 			break;
 
+		case 'T':
+			device_type = atoi(optarg);
+			break;
+
 		case 'R':
 			rotation = (enum Rotation)atoi(optarg);
 			break;
@@ -2376,7 +2437,7 @@ mpu6000_main(int argc, char *argv[])
 
 	 */
 	if (!strcmp(verb, "start")) {
-		mpu6000::start(external_bus, rotation, accel_range);
+		mpu6000::start(external_bus, rotation, accel_range, device_type);
 	}
 
 	if (!strcmp(verb, "stop")) {

src/drivers/px4flow/px4flow.cpp

@@ -694,7 +694,9 @@ start()
 #ifdef PX4_I2C_BUS_ESC
 			PX4_I2C_BUS_ESC,
 #endif
+#ifdef PX4_I2C_BUS_ONBOARD
 			PX4_I2C_BUS_ONBOARD,
+#endif
 			-1
 		};
 

src/drivers/px4fmu/fmu.cpp

@@ -71,6 +71,11 @@
 #include <drivers/drv_mixer.h>
 #include <drivers/drv_rc_input.h>
 
+#include <lib/rc/sbus.h>
+#include <lib/rc/dsm.h>
+#include <lib/rc/st24.h>
+#include <lib/rc/sumd.h>
+
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_0.h>
 #include <uORB/topics/actuator_controls_1.h>
@@ -89,7 +94,7 @@
  * This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side
  */
 
-#define CONTROL_INPUT_DROP_LIMIT_MS		20
+#define CONTROL_INPUT_DROP_LIMIT_US		1800
 #define NAN_VALUE	(0.0f/0.0f)
 
 class PX4FMU : public device::CDev
@@ -121,7 +126,7 @@ private:
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	static const unsigned _max_actuators = 4;
 #endif
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
 	static const unsigned _max_actuators = 6;
 #endif
 #if defined(CONFIG_ARCH_BOARD_AEROCORE)
@@ -141,6 +146,8 @@ private:
 	orb_advert_t	_outputs_pub;
 	unsigned	_num_outputs;
 	int		_class_instance;
+	int		_sbus_fd;
+	int		_dsm_fd;
 
 	volatile bool	_initialized;
 	bool		_servo_armed;
@@ -232,6 +239,17 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
 	{GPIO_VDD_5V_HIPOWER_OC, 0,                       0},
 	{GPIO_VDD_5V_PERIPH_OC,  0,                       0},
 #endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
+	{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0},
+	{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0},
+	{GPIO_GPIO2_INPUT,       GPIO_GPIO2_OUTPUT,       0},
+	{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0},
+	{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0},
+	{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0},
+
+	{0,                      GPIO_VDD_3V3_SENSORS_EN, 0},
+	{GPIO_VDD_BRICK_VALID,   0,                       0},
+#endif
 #if defined(CONFIG_ARCH_BOARD_AEROCORE)
 	/* AeroCore breaks out User GPIOs on J11 */
 	{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0},
@@ -274,6 +292,8 @@ PX4FMU::PX4FMU() :
 	_outputs_pub(nullptr),
 	_num_outputs(0),
 	_class_instance(0),
+	_sbus_fd(-1),
+	_dsm_fd(-1),
 	_initialized(false),
 	_servo_armed(false),
 	_pwm_on(false),
@@ -307,6 +327,11 @@ PX4FMU::PX4FMU() :
 	_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
 #endif
 
+#ifdef GPIO_SBUS_INV
+	// this board has a GPIO to control SBUS inversion
+	stm32_configgpio(GPIO_SBUS_INV);
+#endif
+
 	/* only enable this during development */
 	_debug_enabled = false;
 }
@@ -569,7 +594,7 @@ PX4FMU::update_pwm_rev_mask()
 void
 PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues)
 {
-	actuator_outputs_s outputs;
+	actuator_outputs_s outputs = {};
 	outputs.noutputs = numvalues;
 	outputs.timestamp = hrt_absolute_time();
 
@@ -620,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;
 	}
 
@@ -776,10 +810,47 @@ PX4FMU::cycle()
 		update_pwm_rev_mask();
 	}
 
+	bool rc_updated = false;
+
+#ifdef SBUS_SERIAL_PORT
+	bool sbus_failsafe, sbus_frame_drop;
+	uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS];
+	uint16_t raw_rc_count;
+	bool sbus_updated = sbus_input(_sbus_fd, &raw_rc_values[0], &raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
+				       input_rc_s::RC_INPUT_MAX_CHANNELS);
+
+	if (sbus_updated) {
+		// we have a new PPM frame. Publish it.
+		_rc_in.channel_count = raw_rc_count;
+
+		if (_rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+			_rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
+		}
+
+		for (uint8_t i = 0; i < _rc_in.channel_count; i++) {
+			_rc_in.values[i] = raw_rc_values[i];
+		}
+
+		_rc_in.timestamp_publication = hrt_absolute_time();
+		_rc_in.timestamp_last_signal = _rc_in.timestamp_publication;
+
+		_rc_in.rc_ppm_frame_length = 0;
+		_rc_in.rssi = (!sbus_frame_drop) ? RC_INPUT_RSSI_MAX : 0;
+		_rc_in.rc_failsafe = false;
+		_rc_in.rc_lost = false;
+		_rc_in.rc_lost_frame_count = 0;
+		_rc_in.rc_total_frame_count = 0;
+
+		rc_updated = true;
+	}
+
+#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;
 
@@ -801,6 +872,12 @@ PX4FMU::cycle()
 		_rc_in.rc_lost_frame_count = 0;
 		_rc_in.rc_total_frame_count = 0;
 
+		rc_updated = true;
+	}
+
+#endif
+
+	if (rc_updated) {
 		/* lazily advertise on first publication */
 		if (_to_input_rc == nullptr) {
 			_to_input_rc = orb_advertise(ORB_ID(input_rc), &_rc_in);
@@ -810,8 +887,7 @@ PX4FMU::cycle()
 		}
 	}
 
-#endif
-	work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_MS * 1000));
+	work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(CONTROL_INPUT_DROP_LIMIT_US));
 }
 
 void PX4FMU::work_stop()
@@ -1266,7 +1342,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 				set_mode(MODE_4PWM);
 				break;
 
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) ||  defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
 
 			case 6:
 				set_mode(MODE_6PWM);
@@ -1479,6 +1555,79 @@ PX4FMU::sensor_reset(int ms)
 	// stm32_configgpio(GPIO_ACCEL_DRDY);
 	// stm32_configgpio(GPIO_EXTI_MPU_DRDY);
 
+#endif
+#endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
+
+	if (ms < 1) {
+		ms = 1;
+	}
+
+	/* disable SPI bus */
+	stm32_configgpio(GPIO_SPI_CS_OFF_MPU9250);
+	stm32_configgpio(GPIO_SPI_CS_OFF_HMC5983);
+	stm32_configgpio(GPIO_SPI_CS_OFF_MS5611);
+	stm32_configgpio(GPIO_SPI_CS_OFF_ICM_20608_G);
+
+	stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 0);
+	stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 0);
+
+	stm32_configgpio(GPIO_SPI1_SCK_OFF);
+	stm32_configgpio(GPIO_SPI1_MISO_OFF);
+	stm32_configgpio(GPIO_SPI1_MOSI_OFF);
+
+	stm32_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
+	stm32_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
+
+	stm32_configgpio(GPIO_DRDY_OFF_MPU9250);
+	stm32_configgpio(GPIO_DRDY_OFF_HMC5983);
+	stm32_configgpio(GPIO_DRDY_OFF_ICM_20608_G);
+
+	stm32_gpiowrite(GPIO_DRDY_OFF_MPU9250, 0);
+	stm32_gpiowrite(GPIO_DRDY_OFF_HMC5983, 0);
+	stm32_gpiowrite(GPIO_DRDY_OFF_ICM_20608_G, 0);
+
+	/* set the sensor rail off */
+	stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
+	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
+
+	/* wait for the sensor rail to reach GND */
+	usleep(ms * 1000);
+	warnx("reset done, %d ms", ms);
+
+	/* re-enable power */
+
+	/* switch the sensor rail back on */
+	stm32_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
+
+	/* wait a bit before starting SPI, different times didn't influence results */
+	usleep(100);
+
+	/* reconfigure the SPI pins */
+#ifdef CONFIG_STM32_SPI1
+	stm32_configgpio(GPIO_SPI_CS_MPU9250);
+	stm32_configgpio(GPIO_SPI_CS_HMC5983);
+	stm32_configgpio(GPIO_SPI_CS_MS5611);
+	stm32_configgpio(GPIO_SPI_CS_ICM_20608_G);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_OFF_MPU9250, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_OFF_HMC5983, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_OFF_MS5611, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_OFF_ICM_20608_G, 1);
+
+	// // XXX bring up the EXTI pins again
+	// stm32_configgpio(GPIO_GYRO_DRDY);
+	// stm32_configgpio(GPIO_MAG_DRDY);
+	// stm32_configgpio(GPIO_ACCEL_DRDY);
+	// stm32_configgpio(GPIO_EXTI_MPU_DRDY);
+
 #endif
 #endif
 }
@@ -1505,6 +1654,31 @@ PX4FMU::peripheral_reset(int ms)
 	/* switch the peripheral rail back on */
 	stm32_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0);
 #endif
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
+
+	if (ms < 1) {
+		ms = 10;
+	}
+
+	/* set the peripheral rails off */
+	stm32_configgpio(GPIO_PERIPH_3V3_EN);
+
+	stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 0);
+
+	bool last =  stm32_gpioread(GPIO_SPEKTRUM_POWER);
+	/* Keep Spektum on to discharge rail*/
+	stm32_gpiowrite(GPIO_SPEKTRUM_POWER, 1);
+
+	/* wait for the peripheral rail to reach GND */
+	usleep(ms * 1000);
+	warnx("reset done, %d ms", ms);
+
+	/* re-enable power */
+
+	/* switch the peripheral rail back on */
+	stm32_gpiowrite(GPIO_SPEKTRUM_POWER, last);
+	stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1);
+#endif
 }
 
 void
@@ -1696,7 +1870,7 @@ fmu_new_mode(PortMode new_mode)
 		/* select 4-pin PWM mode */
 		servo_mode = PX4FMU::MODE_4PWM;
 #endif
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) ||  defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
 		servo_mode = PX4FMU::MODE_6PWM;
 #endif
 #if defined(CONFIG_ARCH_BOARD_AEROCORE)
@@ -1704,7 +1878,7 @@ fmu_new_mode(PortMode new_mode)
 #endif
 		break;
 
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) ||  defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
 
 	case PORT_PWM4:
 		/* select 4-pin PWM mode */
@@ -2012,7 +2186,7 @@ fmu_main(int argc, char *argv[])
 	} else if (!strcmp(verb, "mode_pwm")) {
 		new_mode = PORT_FULL_PWM;
 
-#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) ||  defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
 
 	} else if (!strcmp(verb, "mode_pwm4")) {
 		new_mode = PORT_PWM4;
@@ -2101,7 +2275,7 @@ fmu_main(int argc, char *argv[])
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	fprintf(stderr,
 		"  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test, fake, sensor_reset, id\n");
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE)
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4) || defined(CONFIG_ARCH_BOARD_AEROCORE)
 	fprintf(stderr, "  mode_gpio, mode_pwm, mode_pwm4, test, sensor_reset [milliseconds], i2c <bus> <hz>\n");
 #endif
 	exit(1);

src/drivers/stm32/adc/adc.cpp

@@ -348,6 +348,40 @@ ADC::update_system_power(void)
 	}
 
 #endif // CONFIG_ARCH_BOARD_PX4FMU_V2
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4
+	system_power_s system_power;
+	system_power.timestamp = hrt_absolute_time();
+
+	system_power.voltage5V_v = 0;
+
+	for (unsigned i = 0; i < _channel_count; i++) {
+		if (_samples[i].am_channel == 4) {
+			// it is 2:1 scaled
+			system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096);
+		}
+	}
+
+	// these are not ADC related, but it is convenient to
+	// publish these to the same topic
+	system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS);
+
+	// note that the valid pins are active High
+	system_power.brick_valid   = stm32_gpioread(GPIO_VDD_BRICK_VALID);
+	system_power.servo_valid   = 1;
+
+	// OC pins are not supported
+	system_power.periph_5V_OC  = 0;
+	system_power.hipower_5V_OC = 0;
+
+	/* lazily publish */
+	if (_to_system_power != nullptr) {
+		orb_publish(ORB_ID(system_power), _to_system_power, &system_power);
+
+	} else {
+		_to_system_power = orb_advertise(ORB_ID(system_power), &system_power);
+	}
+
+#endif // CONFIG_ARCH_BOARD_PX4FMU_V4
 }
 
 uint16_t

src/drivers/stm32/drv_hrt.c

@@ -221,6 +221,7 @@
 /*
  * Specific registers and bits used by HRT sub-functions
  */
+/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/
 #if HRT_TIMER_CHANNEL == 1
 # define rCCR_HRT	rCCR1			/* compare register for HRT */
 # define DIER_HRT	GTIM_DIER_CC1IE		/* interrupt enable for HRT */
@@ -294,7 +295,7 @@ static void		hrt_call_invoke(void);
 #  define GTIM_CCER_CC4NP 0
 #  define PPM_EDGE_FLIP
 # endif
-
+/* FIXME! There is an interaction in the CCMR registers that prevents using Chan 1 as the timer and chan 2 as the PPM*/
 # if HRT_PPM_CHANNEL == 1
 #  define rCCR_PPM	rCCR1			/* capture register for PPM */
 #  define DIER_PPM	GTIM_DIER_CC1IE		/* capture interrupt (non-DMA mode) */

src/firmware/nuttx/CMakeLists.txt

@@ -41,13 +41,17 @@ add_custom_target(check_weak
 
 if(NOT ${BOARD} STREQUAL "sim")
 
+	if (config_io_board)
+		set(extras "${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin")
+	endif()
 
 	px4_nuttx_add_romfs(OUT romfs
 		ROOT ROMFS/px4fmu_common
-		EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin
+		EXTRAS ${extras}
 		)
-	add_dependencies(romfs fw_io)
-
+	if (config_io_board)
+	    add_dependencies(romfs fw_io)
+    endif()
 	set(fw_file
 		${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4)
 

src/lib/rc/CMakeLists.txt

@@ -37,6 +37,8 @@ px4_add_module(
 	SRCS
 		st24.c
 		sumd.c
+		sbus.c
+		dsm.c
 	DEPENDS
 		platforms__common
 	)

src/lib/rc/dsm.c

@@ -46,9 +46,16 @@
 #include <unistd.h>
 #include <termios.h>
 
+#include "dsm.h"
 #include <drivers/drv_hrt.h>
 
-#include "px4io.h"
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+#include <px4io.h>
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+#include <px4io.h>
+#endif
 
 #define DSM_FRAME_SIZE		16		/**<DSM frame size in bytes*/
 #define DSM_FRAME_CHANNELS	7		/**<Max supported DSM channels*/
@@ -61,6 +68,9 @@ static unsigned dsm_partial_frame_count;	/**< Count of bytes received for curren
 static unsigned dsm_channel_shift;			/**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
 static unsigned dsm_frame_drops;			/**< Count of incomplete DSM frames */
 
+static bool
+dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values);
+
 /**
  * Attempt to decode a single channel raw channel datum
  *
@@ -185,18 +195,18 @@ dsm_guess_format(bool reset)
 
 	if ((votes11 == 1) && (votes10 == 0)) {
 		dsm_channel_shift = 11;
-		debug("DSM: 11-bit format");
+		//debug("DSM: 11-bit format");
 		return;
 	}
 
 	if ((votes10 == 1) && (votes11 == 0)) {
 		dsm_channel_shift = 10;
-		debug("DSM: 10-bit format");
+		//debug("DSM: 10-bit format");
 		return;
 	}
 
 	/* call ourselves to reset our state ... we have to try again */
-	debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
+	//debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
 	dsm_guess_format(true);
 }
 
@@ -237,11 +247,11 @@ dsm_init(const char *device)
 		/* reset the format detector */
 		dsm_guess_format(true);
 
-		debug("DSM: ready");
+		//debug("DSM: ready");
 
 	} else {
 
-		debug("DSM: open failed");
+		//debug("DSM: open failed");
 
 	}
 
@@ -257,11 +267,8 @@ dsm_init(const char *device)
 void
 dsm_bind(uint16_t cmd, int pulses)
 {
-#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)
-#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM
+#if !defined(GPIO_USART1_RX_SPEKTRUM)
 #else
-	const uint32_t usart1RxAsOutp =
-		GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
 
 	if (dsm_fd < 0) {
 		return;
@@ -272,9 +279,9 @@ dsm_bind(uint16_t cmd, int pulses)
 	case dsm_bind_power_down:
 
 		/*power down DSM satellite*/
-#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+#if defined(CONFIG_ARCH_BOARD_PX4IO_V1)
 		POWER_RELAY1(0);
-#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
+#elif defined(CONFIG_ARCH_BOARD_PX4IO_V2)
 		POWER_SPEKTRUM(0);
 #endif
 		break;
@@ -282,9 +289,9 @@ dsm_bind(uint16_t cmd, int pulses)
 	case dsm_bind_power_up:
 
 		/*power up DSM satellite*/
-#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+#if defined(CONFIG_ARCH_BOARD_PX4IO_V1)
 		POWER_RELAY1(1);
-#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */
+#elif defined(CONFIG_ARCH_BOARD_PX4IO_V2)
 		POWER_SPEKTRUM(1);
 #endif
 		dsm_guess_format(true);
@@ -293,7 +300,7 @@ dsm_bind(uint16_t cmd, int pulses)
 	case dsm_bind_set_rx_out:
 
 		/*Set UART RX pin to active output mode*/
-		stm32_configgpio(usart1RxAsOutp);
+		stm32_configgpio(GPIO_USART1_RX_SPEKTRUM);
 		break;
 
 	case dsm_bind_send_pulses:
@@ -301,9 +308,9 @@ dsm_bind(uint16_t cmd, int pulses)
 		/*Pulse RX pin a number of times*/
 		for (int i = 0; i < pulses; i++) {
 			up_udelay(120);
-			stm32_gpiowrite(usart1RxAsOutp, false);
+			stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, false);
 			up_udelay(120);
-			stm32_gpiowrite(usart1RxAsOutp, true);
+			stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, true);
 		}
 
 		break;
@@ -327,8 +334,8 @@ dsm_bind(uint16_t cmd, int pulses)
  * @param[out] num_values pointer to number of raw channel values returned
  * @return true=DSM frame successfully decoded, false=no update
  */
-static bool
-dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
+bool
+dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values)
 {
 	/*
 	debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
@@ -374,7 +381,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		}
 
 		/* ignore channels out of range */
-		if (channel >= PX4IO_RC_INPUT_CHANNELS) {
+		if (channel >= max_values) {
 			continue;
 		}
 
@@ -473,7 +480,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
  * @return true=decoded raw channel values updated, false=no update
  */
 bool
-dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes)
+dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -523,5 +530,5 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by
 	 * decode it.
 	 */
 	dsm_partial_frame_count = 0;
-	return dsm_decode(now, values, num_values);
+	return dsm_decode(now, values, num_values, max_values);
 }

src/lib/rc/dsm.h

@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *	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
+ * 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 dsm.h
+ *
+ * RC protocol definition for Spektrum RC
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+__EXPORT int	dsm_init(const char *device);
+__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);
+
+__END_DECLS

src/lib/rc/sbus.c

@@ -43,15 +43,9 @@
 #include <unistd.h>
 #include <termios.h>
 
-#include <systemlib/ppm_decode.h>
-
+#include "sbus.h"
 #include <drivers/drv_hrt.h>
 
-#define DEBUG
-#include "px4io.h"
-#include "protocol.h"
-#include "debug.h"
-
 #define SBUS_FRAME_SIZE		25
 #define SBUS_INPUT_CHANNELS	16
 #define SBUS_FLAGS_BYTE		23
@@ -77,8 +71,6 @@
 #define SBUS_SCALE_FACTOR ((SBUS_TARGET_MAX - SBUS_TARGET_MIN) / (SBUS_RANGE_MAX - SBUS_RANGE_MIN))
 #define SBUS_SCALE_OFFSET (int)(SBUS_TARGET_MIN - (SBUS_SCALE_FACTOR * SBUS_RANGE_MIN + 0.5f))
 
-static int sbus_fd = -1;
-
 static hrt_abstime last_rx_time;
 static hrt_abstime last_frame_time;
 static hrt_abstime last_txframe_time = 0;
@@ -93,11 +85,9 @@ static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_
 			bool *sbus_frame_drop, uint16_t max_channels);
 
 int
-sbus_init(const char *device)
+sbus_init(const char *device, bool singlewire)
 {
-	if (sbus_fd < 0) {
-		sbus_fd = open(device, O_RDWR | O_NONBLOCK);
-	}
+	int sbus_fd = open(device, O_RDWR | O_NONBLOCK);
 
 	if (sbus_fd >= 0) {
 		struct termios t;
@@ -108,21 +98,24 @@ sbus_init(const char *device)
 		t.c_cflag |= (CSTOPB | PARENB);
 		tcsetattr(sbus_fd, TCSANOW, &t);
 
+		if (singlewire) {
+			/* only defined in configs capable of IOCTL */
+#ifdef SBUS_SERIAL_PORT
+			ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
+#endif
+		}
+
 		/* initialise the decoder */
 		partial_frame_count = 0;
 		last_rx_time = hrt_absolute_time();
 
-		debug("S.Bus: ready");
-
-	} else {
-		debug("S.Bus: open failed");
 	}
 
 	return sbus_fd;
 }
 
 void
-sbus1_output(uint16_t *values, uint16_t num_values)
+sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values)
 {
 	uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
 	uint8_t offset = 0;
@@ -161,14 +154,15 @@ sbus1_output(uint16_t *values, uint16_t num_values)
 	}
 }
 void
-sbus2_output(uint16_t *values, uint16_t num_values)
+sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values)
 {
 	char b = 'B';
 	write(sbus_fd, &b, 1);
 }
 
 bool
-sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
+sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop,
+	   uint16_t max_channels)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -180,8 +174,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
 	 * The minimum frame spacing is 7ms; with 25 bytes at 100000bps
 	 * frame transmission time is ~2ms.
 	 *
-	 * We expect to only be called when bytes arrive for processing,
-	 * and if an interval of more than 3ms passes between calls,
+	 * If an interval of more than 4ms passes between calls,
 	 * the first byte we read will be the first byte of a frame.
 	 *
 	 * In the case where byte(s) are dropped from a frame, this also
@@ -190,7 +183,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sb
 	 */
 	now = hrt_absolute_time();
 
-	if ((now - last_rx_time) > 3000) {
+	if ((now - last_rx_time) > 4000) {
 		if (partial_frame_count > 0) {
 			sbus_frame_drops++;
 			partial_frame_count = 0;
@@ -331,7 +324,7 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool
 	}
 
 	/* decode switch channels if data fields are wide enough */
-	if (PX4IO_RC_INPUT_CHANNELS > 17 && chancount > 15) {
+	if (max_values > 17 && chancount > 15) {
 		chancount = 18;
 
 		/* channel 17 (index 16) */

src/lib/rc/sbus.h

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *	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
+ * 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 sbus.h
+ *
+ * RC protocol definition for S.BUS
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+__BEGIN_DECLS
+
+__EXPORT int	sbus_init(const char *device, bool singlewire);
+
+/**
+ * Parse serial bytes on the S.BUS bus
+ *
+ * The S.bus protocol doesn't provide reliable framing,
+ * so we detect frame boundaries by the inter-frame delay.
+ *
+ * The minimum frame spacing is 7ms; with 25 bytes at 100000bps
+ * frame transmission time is ~2ms.
+ *
+ * If an interval of more than 4ms (7 ms frame spacing plus margin)
+ * passes between calls, the first byte we read will be the first
+ * byte of a frame.
+ *
+ * In the case where byte(s) are dropped from a frame, this also
+ * provides a degree of protection. Of course, it would be better
+ * if we didn't drop bytes...
+ */
+__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);
+__EXPORT void	sbus1_output(int sbus_fd, uint16_t *values, uint16_t num_values);
+__EXPORT void	sbus2_output(int sbus_fd, uint16_t *values, uint16_t num_values);
+
+__END_DECLS

src/lib/rc/sumd.h

@@ -101,8 +101,8 @@ uint8_t sumd_crc8(uint8_t crc, uint8_t value);
 __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
 				 uint16_t *channels, uint16_t max_chan_count);
 */
-int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
-		uint16_t *channels, uint16_t max_chan_count);
+__EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
+			 uint16_t *channels, uint16_t max_chan_count);
 
 
 __END_DECLS

src/lib/version/version.h

@@ -51,6 +51,10 @@
 #define	HW_ARCH "PX4FMU_V2"
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V4
+#define	HW_ARCH "PX4FMU_V4"
+#endif
+
 #ifdef CONFIG_ARCH_BOARD_AEROCORE
 #define	HW_ARCH "AEROCORE"
 #endif

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -59,6 +59,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/att_pos_mocap.h>
+#include <uORB/topics/airspeed.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
@@ -122,6 +123,7 @@ private:
 	int		_params_sub = -1;
 	int		_vision_sub = -1;
 	int		_mocap_sub = -1;
+	int		_airspeed_sub = -1;
 	int		_global_pos_sub = -1;
 	orb_advert_t	_att_pub = nullptr;
 	orb_advert_t	_ctrl_state_pub = nullptr;
@@ -161,6 +163,8 @@ private:
 	att_pos_mocap_s _mocap = {};
 	Vector<3>	_mocap_hdg;
 
+	airspeed_s _airspeed = {};
+
 	Quaternion	_q;
 	Vector<3>	_rates;
 	Vector<3>	_gyro_bias;
@@ -202,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),
@@ -258,7 +264,7 @@ int AttitudeEstimatorQ::start()
 	_control_task = px4_task_spawn_cmd("attitude_estimator_q",
 					   SCHED_DEFAULT,
 					   SCHED_PRIORITY_MAX - 5,
-					   2100,
+					   2400,
 					   (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
 					   nullptr);
 
@@ -299,6 +305,8 @@ void AttitudeEstimatorQ::task_main()
 	_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
 	_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
 
+	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 
@@ -334,6 +342,10 @@ void AttitudeEstimatorQ::task_main()
 		// Update sensors
 		sensor_combined_s sensors;
 
+		int best_gyro = 0;
+		int best_accel = 0;
+		int best_mag = 0;
+
 		if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
 			// Feed validator with recent sensor data
 
@@ -370,8 +382,6 @@ void AttitudeEstimatorQ::task_main()
 				}
 			}
 
-			int best_gyro, best_accel, best_mag;
-
 			// Get best measurement values
 			hrt_abstime curr_time = hrt_absolute_time();
 			_gyro.set(_voter_gyro.get_best(curr_time, &best_gyro));
@@ -488,6 +498,14 @@ void AttitudeEstimatorQ::task_main()
 			_mocap_hdg = Rmoc.transposed() * v;
 		}
 
+		// Update airspeed
+		bool airspeed_updated = false;
+		orb_check(_airspeed_sub, &airspeed_updated);
+
+		if (airspeed_updated) {
+			orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+		}
+
 		// Check for timeouts on data
 		if (_ext_hdg_mode == 1) {
 			_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
@@ -601,6 +619,9 @@ void AttitudeEstimatorQ::task_main()
 
 		ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
 
+		/* Airspeed - take airspeed measurement directly here as no wind is estimated */
+		ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
+
 		/* Publish to control state topic */
 		if (_ctrl_state_pub == nullptr) {
 			_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
@@ -723,6 +744,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/commander/commander.cpp

@@ -1645,7 +1645,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
 			status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
 
-			/* Make sure that this is only adjusted if vehicle really is of type vtol*/
+			/* Make sure that this is only adjusted if vehicle really is of type vtol */
 			if (is_vtol(&status)) {
 				status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
 				status.in_transition_mode = vtol_status.vtol_in_trans_mode;
@@ -2365,7 +2365,7 @@ int commander_thread_main(int argc, char *argv[])
 			if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
 			     status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE ||
 			     status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
-			     status.main_state !=vehicle_status_s::MAIN_STATE_STAB ||
+			     status.main_state ==vehicle_status_s::MAIN_STATE_STAB ||
 			     status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
 			     status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) &&
 			    ((status.rc_signal_lost && status.gps_failure) ||

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -856,7 +856,9 @@ void AttitudePositionEstimatorEKF::publishControlState()
 	/* Accelerations in Body Frame */
 	_ctrl_state.x_acc = _ekf->accel.x;
 	_ctrl_state.y_acc = _ekf->accel.y;
-	_ctrl_state.z_acc = _ekf->accel.z;
+	_ctrl_state.z_acc = _ekf->accel.z - _ekf->states[13];
+
+	_ctrl_state.horz_acc_mag = _ekf->getAccNavMagHorizontal();
 
 	/* Velocity in Body Frame */
 	Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]);

src/modules/ekf_att_pos_estimator/estimator_22states.h

@@ -382,6 +382,8 @@ public:
 
     void get_covariance(float c[28]);
 
+    float getAccNavMagHorizontal() { return _accNavMagHorizontal; }
+
 protected:
 
     /**

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -59,6 +59,7 @@
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/fw_virtual_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_virtual_fw.h>
@@ -142,6 +143,7 @@ private:
 
 	orb_id_t _rates_sp_id;	// pointer to correct rates setpoint uORB metadata structure
 	orb_id_t _actuators_id;	// pointer to correct actuator controls0 uORB metadata structure
+	orb_id_t _attitude_setpoint_id;
 
 	struct control_state_s				_ctrl_state;	/**< control state */
 	struct accel_report				_accel;			/**< body frame accelerations */
@@ -360,6 +362,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
 	_rates_sp_id(0),
 	_actuators_id(0),
+	_attitude_setpoint_id(0),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -633,9 +636,11 @@ FixedwingAttitudeControl::vehicle_status_poll()
 			if (_vehicle_status.is_vtol) {
 				_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
 				_actuators_id = ORB_ID(actuator_controls_virtual_fw);
+				_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
 			} else {
 				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
 				_actuators_id = ORB_ID(actuator_controls_0);
+				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
 			}
 		}
 	}
@@ -1015,15 +1020,12 @@ FixedwingAttitudeControl::task_main()
 					att_sp.thrust = throttle_sp;
 
 					/* lazily publish the setpoint only once available */
-					if (!_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
-						if (_attitude_sp_pub != nullptr) {
-							/* publish the attitude setpoint */
-							orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
-
-						} else {
-							/* advertise and publish */
-							_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
-						}
+					if (_attitude_sp_pub != nullptr) {
+						/* publish the attitude setpoint */
+						orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp);
+					} else if (_attitude_setpoint_id) {
+						/* advertise and publish */
+						_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp);
 					}
 				}
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -72,6 +72,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/fw_virtual_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
@@ -166,6 +167,8 @@ private:
 	orb_advert_t	_tecs_status_pub;		/**< TECS status publication */
 	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */
 
+	orb_id_t _attitude_setpoint_id;
+
 	struct control_state_s				_ctrl_state;			/**< control state */
 	struct vehicle_attitude_setpoint_s		_att_sp;			/**< vehicle attitude setpoint */
 	struct navigation_capabilities_s		_nav_capabilities;		/**< navigation capabilities */
@@ -506,6 +509,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_tecs_status_pub(nullptr),
 	_nav_capabilities_pub(nullptr),
 
+/* publication ID */
+	_attitude_setpoint_id(0),
+
 /* states */
 	_ctrl_state(),
 	_att_sp(),
@@ -766,6 +772,14 @@ FixedwingPositionControl::vehicle_status_poll()
 
 	if (updated) {
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+		/* set correct uORB ID, depending on if vehicle is VTOL or not */
+		if (!_attitude_setpoint_id) {
+			if (_vehicle_status.is_vtol) {
+				_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
+			} else {
+				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
+			}
+		}
 	}
 }
 
@@ -1990,13 +2004,13 @@ FixedwingPositionControl::task_main()
 				_att_sp.timestamp = hrt_absolute_time();
 
 				/* lazily publish the setpoint only once available */
-				if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
+				if (_attitude_sp_pub != nullptr) {
 					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
+					orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &_att_sp);
 
-				} else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
+				} else if (_attitude_setpoint_id) {
 					/* advertise and publish */
-					_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+					_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
 				}
 
 				/* XXX check if radius makes sense here */

src/modules/gpio_led/gpio_led.c

@@ -89,7 +89,7 @@ int gpio_led_main(int argc, char *argv[])
 		     "\t\tr2\tPX4IO RELAY2"
 		    );
 #endif
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
 		errx(1, "usage: gpio_led {start|stop} [-p <n>]\n"
 		     "\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
 		    );
@@ -111,7 +111,7 @@ int gpio_led_main(int argc, char *argv[])
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 			char *pin_name = "PX4FMU GPIO_EXT1";
 #endif
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
 			char pin_name[] = "AUX OUT 1";
 #endif
 
@@ -154,7 +154,7 @@ int gpio_led_main(int argc, char *argv[])
 					}
 
 #endif
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
 					unsigned int n = strtoul(argv[3], NULL, 10);
 
 					if (n >= 1 && n <= 6) {
@@ -207,6 +207,8 @@ void gpio_led_start(FAR void *arg)
 
 	char *gpio_dev;
 
+#if defined(PX4IO_DEVICE_PATH)
+
 	if (priv->use_io) {
 		gpio_dev = PX4IO_DEVICE_PATH;
 
@@ -214,6 +216,10 @@ void gpio_led_start(FAR void *arg)
 		gpio_dev = PX4FMU_DEVICE_PATH;
 	}
 
+#else
+	gpio_dev = PX4FMU_DEVICE_PATH;
+#endif
+
 	/* open GPIO device */
 	priv->gpio_fd = open(gpio_dev, 0);
 

src/modules/land_detector/CMakeLists.txt

@@ -41,6 +41,7 @@ px4_add_module(
 		LandDetector.cpp
 		MulticopterLandDetector.cpp
 		FixedwingLandDetector.cpp
+		VtolLandDetector.cpp
 	DEPENDS
 		platforms__common
 	)

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -36,6 +36,7 @@
  * Land detection algorithm for fixedwings
  *
  * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
 #include "FixedwingLandDetector.h"
@@ -48,42 +49,44 @@
 FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
 	_paramHandle(),
 	_params(),
-	_vehicleLocalPositionSub(-1),
-	_vehicleLocalPosition( {}),
-		       _airspeedSub(-1),
-		       _vehicleStatusSub(-1),
-		       _armingSub(-1),
-		       _airspeed{},
-		       _vehicleStatus{},
-		       _arming{},
-		       _parameterSub(-1),
-		       _velocity_xy_filtered(0.0f),
-		       _velocity_z_filtered(0.0f),
-		       _airspeed_filtered(0.0f),
-		       _landDetectTrigger(0)
+	_controlStateSub(-1),
+	_vehicleStatusSub(-1),
+	_armingSub(-1),
+	_airspeedSub(-1),
+	_controlState{},
+	_vehicleStatus{},
+	_arming{},
+	_airspeed{},
+	_parameterSub(-1),
+	_velocity_xy_filtered(0.0f),
+	_velocity_z_filtered(0.0f),
+	_airspeed_filtered(0.0f),
+	_accel_horz_lp(0.0f),
+	_landDetectTrigger(0)
 {
 	_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
 	_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
 	_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
+	_paramHandle.maxIntVelocity = param_find("LNDFW_VELI_MAX");
 }
 
 void FixedwingLandDetector::initialize()
 {
 	// Subscribe to local position and airspeed data
-	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
-	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
+	_controlStateSub = orb_subscribe(ORB_ID(control_state));
 	_vehicleStatusSub = orb_subscribe(ORB_ID(vehicle_status));
 	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
+	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
 
 	updateParameterCache(true);
 }
 
 void FixedwingLandDetector::updateSubscriptions()
 {
-	orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
-	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
+	orb_update(ORB_ID(control_state), _controlStateSub, &_controlState);
 	orb_update(ORB_ID(vehicle_status), _vehicleStatusSub, &_vehicleStatus);
 	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
+	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
 }
 
 bool FixedwingLandDetector::update()
@@ -99,29 +102,33 @@ bool FixedwingLandDetector::update()
 	const uint64_t now = hrt_absolute_time();
 	bool landDetected = false;
 
-	if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
-		float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx *
-				_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
+	if (hrt_elapsed_time(&_controlState.timestamp) < 500 * 1000) {
+		float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_controlState.x_vel *
+				_controlState.x_vel + _controlState.y_vel * _controlState.y_vel);
 
 		if (PX4_ISFINITE(val)) {
 			_velocity_xy_filtered = val;
 		}
 
-		val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz);
+		val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_controlState.z_vel);
 
 		if (PX4_ISFINITE(val)) {
 			_velocity_z_filtered = val;
 		}
-	}
 
-	if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
 		_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
+
+		// a leaking lowpass prevents biases from building up, but
+		// gives a mostly correct response for short impulses
+		_accel_horz_lp = _accel_horz_lp * 0.8f + _controlState.horz_acc_mag * 0.18f;
+
 	}
 
 	// crude land detector for fixedwing
 	if (_velocity_xy_filtered < _params.maxVelocity
 	    && _velocity_z_filtered < _params.maxClimbRate
-	    && _airspeed_filtered < _params.maxAirSpeed) {
+	    && _airspeed_filtered < _params.maxAirSpeed
+	    && _accel_horz_lp < _params.maxIntVelocity) {
 
 		// these conditions need to be stable for a period of time before we trust them
 		if (now > _landDetectTrigger) {
@@ -151,5 +158,6 @@ void FixedwingLandDetector::updateParameterCache(const bool force)
 		param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
 		param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
 		param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
+		param_get(_paramHandle.maxIntVelocity, &_params.maxIntVelocity);
 	}
 }

src/modules/land_detector/FixedwingLandDetector.h

@@ -42,11 +42,11 @@
 #define __FIXED_WING_LAND_DETECTOR_H__
 
 #include "LandDetector.h"
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/airspeed.h>
+#include <uORB/topics/control_state.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/airspeed.h>
 #include <systemlib/param/param.h>
 
 class FixedwingLandDetector : public LandDetector
@@ -83,28 +83,31 @@ private:
 		param_t maxVelocity;
 		param_t maxClimbRate;
 		param_t maxAirSpeed;
+		param_t maxIntVelocity;
 	}		_paramHandle;
 
 	struct {
 		float maxVelocity;
 		float maxClimbRate;
 		float maxAirSpeed;
+		float maxIntVelocity;
 	} _params;
 
 private:
-	int					_vehicleLocalPositionSub;	/**< notification of local position */
-	struct vehicle_local_position_s		_vehicleLocalPosition;		/**< the result from local position subscription */
-	int					_airspeedSub;
+	int					_controlStateSub;	/**< notification of local position */
 	int					_vehicleStatusSub;
 	int					_armingSub;
-	struct airspeed_s			_airspeed;
+	int					_airspeedSub;
+	struct control_state_s			_controlState;		/**< the result from local position subscription */
 	struct vehicle_status_s 		_vehicleStatus;
 	struct actuator_armed_s			_arming;
+	struct airspeed_s			_airspeed;
 	int 					_parameterSub;
 
 	float _velocity_xy_filtered;
 	float _velocity_z_filtered;
 	float _airspeed_filtered;
+	float _accel_horz_lp;
 	uint64_t _landDetectTrigger;
 };
 

src/modules/land_detector/LandDetector.cpp

@@ -57,14 +57,14 @@ _work{} {
 
 LandDetector::~LandDetector()
 {
-	work_cancel(LPWORK, &_work);
+	work_cancel(HPWORK, &_work);
 	_taskShouldExit = true;
 }
 
 int LandDetector::start()
 {
 	/* schedule a cycle to start things */
-	work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0);
+	work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0);
 
 	return 0;
 }
@@ -107,10 +107,11 @@ void LandDetector::cycle()
 
 		// publish the land detected broadcast
 		orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected);
+		//warnx("in air status changed: %s", (_landDetected.landed) ? "LANDED" : "TAKEOFF");
 	}
 
 	if (!_taskShouldExit) {
-		work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this,
+		work_queue(HPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this,
 			   USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE));
 	}
 }

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -95,6 +95,13 @@ bool MulticopterLandDetector::update()
 	// first poll for new data from our subscriptions
 	updateSubscriptions();
 
+	updateParameterCache(false);
+
+	return get_landed_state();
+}
+
+bool MulticopterLandDetector::get_landed_state()
+{
 	// only trigger flight conditions if we are armed
 	if (!_arming.armed) {
 		_arming_time = 0;

src/modules/land_detector/MulticopterLandDetector.h

@@ -60,24 +60,29 @@ protected:
 	/**
 	* @brief  polls all subscriptions and pulls any data that has changed
 	**/
-	void updateSubscriptions();
+	virtual void updateSubscriptions();
 
 	/**
 	* @brief Runs one iteration of the land detection algorithm
 	**/
-	bool update() override;
+	virtual bool update() override;
 
 	/**
 	* @brief Initializes the land detection algorithm
 	**/
-	void initialize() override;
+	virtual void initialize() override;
 
-
-private:
 	/**
 	* @brief download and update local parameter cache
 	**/
-	void updateParameterCache(const bool force);
+	virtual void updateParameterCache(const bool force);
+
+	/**
+	* @brief get multicopter landed state
+	**/
+	bool get_landed_state();
+
+private:
 
 	/**
 	* @brief Handles for interesting parameters

src/modules/land_detector/VtolLandDetector.cpp

@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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
+ * 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 VtolLandDetector.cpp
+ * Land detection algorithm for vtol
+ *
+ * @author Roman Bapst <bapstroma@gmail.com>
+ */
+
+#include "VtolLandDetector.h"
+#include <drivers/drv_hrt.h>
+
+VtolLandDetector::VtolLandDetector() : MulticopterLandDetector(),
+	_paramHandle(),
+	_params(),
+	_airspeedSub(-1),
+	_parameterSub(-1),
+	_airspeed{},
+	_was_in_air(false),
+	_airspeed_filtered(0)
+{
+	_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
+}
+
+void VtolLandDetector::initialize()
+{
+	MulticopterLandDetector::initialize();
+	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
+
+	// download parameters
+	updateParameterCache(true);
+}
+
+void VtolLandDetector::updateSubscriptions()
+{
+	MulticopterLandDetector::updateSubscriptions();
+
+	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
+}
+
+bool VtolLandDetector::update()
+{
+	updateSubscriptions();
+	updateParameterCache(false);
+
+	// this is returned from the mutlicopter land detector
+	bool landed = get_landed_state();
+
+	// for vtol we additionally consider airspeed
+	if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
+		_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
+
+	} else {
+		// if airspeed does not update, set it to zero and rely on multicopter land detector
+		_airspeed_filtered = 0.0f;
+	}
+
+	// only consider airspeed if we have been in air before to avoid false
+	// detections in the case of wind on the ground
+	if (_was_in_air && _airspeed_filtered > _params.maxAirSpeed) {
+		landed = false;
+	}
+
+	_was_in_air = !landed;
+
+	return landed;
+}
+
+void VtolLandDetector::updateParameterCache(const bool force)
+{
+	MulticopterLandDetector::updateParameterCache(force);
+
+	bool updated;
+	parameter_update_s paramUpdate;
+
+	orb_check(_parameterSub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
+	}
+
+	if (updated || force) {
+		param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
+	}
+}

src/modules/land_detector/VtolLandDetector.h

@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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
+ * 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 VtolLandDetector.h
+ * Land detection algorithm for vtol
+ *
+ * @author Roman Bapst <bapstr@gmail.com>
+ */
+
+#ifndef __VTOL_LAND_DETECTOR_H__
+#define __VTOL_LAND_DETECTOR_H__
+
+#include "MulticopterLandDetector.h"
+#include <uORB/topics/airspeed.h>
+
+class VtolLandDetector : public MulticopterLandDetector
+{
+public:
+	VtolLandDetector();
+
+
+private:
+	/**
+	* @brief  polls all subscriptions and pulls any data that has changed
+	**/
+	void updateSubscriptions() override;
+
+	/**
+	* @brief Runs one iteration of the land detection algorithm
+	**/
+	bool update() override;
+
+	/**
+	* @brief Initializes the land detection algorithm
+	**/
+	void initialize() override;
+
+	/**
+	* @brief download and update local parameter cache
+	**/
+	void updateParameterCache(const bool force) override;
+
+	/**
+	* @brief Handles for interesting parameters
+	**/
+	struct {
+		param_t maxAirSpeed;
+	}		_paramHandle;
+
+	struct {
+		float maxAirSpeed;
+	} _params;
+
+	int _airspeedSub;
+	int _parameterSub;
+
+	struct airspeed_s _airspeed;
+
+	bool _was_in_air;								/**< indicates whether the vehicle was in the air in the previous iteration */
+	float _airspeed_filtered;						/**< low pass filtered airspeed */
+};
+
+#endif

src/modules/land_detector/land_detector_main.cpp

@@ -53,6 +53,7 @@
 
 #include "FixedwingLandDetector.h"
 #include "MulticopterLandDetector.h"
+#include "VtolLandDetector.h"
 
 //Function prototypes
 static int land_detector_start(const char *mode);
@@ -113,6 +114,9 @@ static int land_detector_start(const char *mode)
 	} else if (!strcmp(mode, "multicopter")) {
 		land_detector_task = new MulticopterLandDetector();
 
+	} else if (!strcmp(mode, "vtol")) {
+		land_detector_task = new VtolLandDetector();
+
 	} else {
 		warnx("[mode] must be either 'fixedwing' or 'multicopter'");
 		return -1;

src/modules/land_detector/land_detector_params.c

@@ -109,6 +109,19 @@ PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f);
  */
 PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f);
 
+/**
+ * Fixedwing max short-term velocity
+ *
+ * Maximum velocity integral in flight direction allowed in the landed state (m/s)
+ *
+ * @min 2
+ * @max 10
+ * @unit m/s
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDFW_VELI_MAX, 4.0f);
+
 /**
  * Airspeed max
  *

src/modules/mavlink/mavlink_main.cpp

@@ -1750,7 +1750,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("VFR_HUD", 20.0f);
 		configure_stream("ATTITUDE", 100.0f);
 		configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
-		configure_stream("RC_CHANNELS", 5.0f);
+		configure_stream("RC_CHANNELS", 10.0f);
 		configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
 		configure_stream("SERVO_OUTPUT_RAW_1", 20.0f);
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);

src/modules/mavlink/mavlink_receiver.cpp

@@ -1324,8 +1324,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		struct airspeed_s airspeed = {};
 
 		float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f);
-		// XXX need to fix this
-		float tas = ias;
+		float tas = calc_true_airspeed_from_indicated(ias, imu.abs_pressure * 100, imu.temperature);
 
 		airspeed.timestamp = timestamp;
 		airspeed.indicated_airspeed_m_s = ias;

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -69,6 +69,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/control_state.h>
+#include <uORB/topics/mc_virtual_attitude_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
@@ -141,6 +142,8 @@ private:
 	orb_advert_t	_local_pos_sp_pub;		/**< vehicle local position setpoint publication */
 	orb_advert_t	_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
 
+	orb_id_t _attitude_setpoint_id;
+
 	struct vehicle_status_s 			_vehicle_status; 	/**< vehicle status */
 	struct control_state_s				_ctrl_state;			/**< vehicle attitude */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
@@ -336,6 +339,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_att_sp_pub(nullptr),
 	_local_pos_sp_pub(nullptr),
 	_global_vel_sp_pub(nullptr),
+	_attitude_setpoint_id(0),
 	_manual_thr_min(this, "MANTHR_MIN"),
 	_manual_thr_max(this, "MANTHR_MAX"),
 	_vel_x_deriv(this, "VELD"),
@@ -533,6 +537,14 @@ MulticopterPositionControl::poll_subscriptions()
 
 	if (updated) {
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+		/* set correct uORB ID, depending on if vehicle is VTOL or not */
+		if (!_attitude_setpoint_id) {
+			if (_vehicle_status.is_vtol) {
+				_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);
+			} else {
+				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
+			}
+		}
 	}
 
 	orb_check(_ctrl_state_sub, &updated);
@@ -1141,6 +1153,16 @@ MulticopterPositionControl::task_main()
 			_run_pos_control = true;
 			_run_alt_control = true;
 
+			// reset the horizontal and vertical position hold flags for non-manual modes
+			// or if position is not controlled
+			if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) {
+				_pos_hold_engaged = false;
+			}
+
+			if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) {
+				_alt_hold_engaged = false;
+			}
+
 			/* select control source */
 			if (_control_mode.flag_control_manual_enabled) {
 				/* manual control */
@@ -1173,10 +1195,9 @@ MulticopterPositionControl::task_main()
 
 				/* publish attitude setpoint */
 				if (_att_sp_pub != nullptr) {
-					orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
-
-				} else {
-					_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+					orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
+				} else if (_attitude_setpoint_id) {
+					_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
 				}
 
 			} else {
@@ -1600,16 +1621,18 @@ MulticopterPositionControl::task_main()
 
 		/* publish attitude setpoint
 		 * Do not publish if offboard is enabled but position/velocity control is disabled,
-		 * in this case the attitude setpoint is published by the mavlink app
+		 * in this case the attitude setpoint is published by the mavlink app. Also do not publish
+		 * if the vehicle is a VTOL and it's just doing a transition (the VTOL attitude control module will generate
+		 * attitude setpoints for the transition).
 		 */
 		if (!(_control_mode.flag_control_offboard_enabled &&
-		      !(_control_mode.flag_control_position_enabled ||
-			_control_mode.flag_control_velocity_enabled))) {
-			if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) {
-				orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
+					!(_control_mode.flag_control_position_enabled ||
+						_control_mode.flag_control_velocity_enabled))) {
 
-			} else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) {
-				_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+			if (_att_sp_pub != nullptr) {
+				orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
+			} else if (_attitude_setpoint_id) {
+				_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
 			}
 		}
 

src/modules/position_estimator_inav/CMakeLists.txt

@@ -30,18 +30,21 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+
+set(MODULE_CFLAGS )
 if(${OS} STREQUAL "nuttx")
 	list(APPEND MODULE_CFLAGS -Wframe-larger-than=4000)
 endif()
+
 px4_add_module(
 	MODULE modules__position_estimator_inav
 	MAIN position_estimator_inav
 	STACK 1200
 	COMPILE_FLAGS ${MODULE_CFLAGS}
 	SRCS
-		position_estimator_inav_main.c
-		position_estimator_inav_params.c
-		inertial_filter.c
+		position_estimator_inav_main.cpp
+		position_estimator_inav_params.cpp
+		inertial_filter.cpp
 	DEPENDS
 		platforms__common
 	)

src/modules/position_estimator_inav/position_estimator_inav_main.cpp

@@ -74,6 +74,7 @@
 #include <drivers/drv_hrt.h>
 #include <platforms/px4_defines.h>
 
+#include <terrain_estimation/terrain_estimator.h>
 #include "position_estimator_inav_params.h"
 #include "inertial_filter.h"
 
@@ -95,7 +96,7 @@ static const hrt_abstime lidar_valid_timeout = 1000000;	// estimate lidar distan
 static const unsigned updates_counter_len = 1000000;
 static const float max_flow = 1.0f;	// max flow value that can be used, rad/s
 
-__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
+extern "C" __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
 
 int position_estimator_inav_thread_main(int argc, char *argv[]);
 
@@ -153,7 +154,7 @@ int position_estimator_inav_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		position_estimator_inav_task = px4_task_spawn_cmd("position_estimator_inav",
-					       SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
+					       SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5300,
 					       position_estimator_inav_thread_main,
 					       (argv && argc > 2) ? (char * const *) &argv[2] : (char * const *) NULL);
 		return 0;
@@ -398,6 +399,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	/* wait for initial baro value */
 	bool wait_baro = true;
 
+	TerrainEstimator *terrain_estimator = new TerrainEstimator();
+
 	thread_running = true;
 
 	while (wait_baro && !thread_should_exit) {
@@ -438,9 +441,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	}
 
 	/* main loop */
-	px4_pollfd_struct_t fds[1] = {
-		{ .fd = vehicle_attitude_sub, .events = POLLIN },
-	};
+	px4_pollfd_struct_t fds[1];
+	fds[0].fd = vehicle_attitude_sub;
+	fds[0].events = POLLIN;
 
 	while (!thread_should_exit) {
 		int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
@@ -527,9 +530,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			orb_check(optical_flow_sub, &updated);
 			orb_check(distance_sensor_sub, &updated2);
 
+			/* update lidar separately, needed by terrain estimator */
+			if (updated2) {
+				orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
+			}
+
 			if (updated && updated2) {
 				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
-				orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
 
 				/* calculate time from previous update */
 //				float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
@@ -1045,7 +1052,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				c += R_gps[j][i] * accel_bias_corr[j];
 			}
 
-			if (isfinite(c)) {
+			if (PX4_ISFINITE(c)) {
 				acc_bias[i] += c * params.w_acc_bias * dt;
 			}
 		}
@@ -1085,7 +1092,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				c += PX4_R(att.R, j, i) * accel_bias_corr[j];
 			}
 
-			if (isfinite(c)) {
+			if (PX4_ISFINITE(c)) {
 				acc_bias[i] += c * params.w_acc_bias * dt;
 			}
 		}
@@ -1110,7 +1117,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				c += PX4_R(att.R, j, i) * accel_bias_corr[j];
 			}
 
-			if (isfinite(c)) {
+			if (PX4_ISFINITE(c)) {
 				acc_bias[i] += c * params.w_acc_bias * dt;
 			}
 		}
@@ -1118,7 +1125,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		/* inertial filter prediction for altitude */
 		inertial_filter_predict(dt, z_est, acc[2]);
 
-		if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
+		if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {
 			write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
 										acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
 										corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
@@ -1145,7 +1152,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p);
 		}
 
-		if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
+		if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {
 			write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
 										acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
 										corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
@@ -1164,7 +1171,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			inertial_filter_predict(dt, x_est, acc[0]);
 			inertial_filter_predict(dt, y_est, acc[1]);
 
-			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
+			if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {
 				write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
 										acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
 										corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
@@ -1211,7 +1218,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p);
 			}
 
-			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
+			if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {
 				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
 										acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
 										corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
@@ -1232,6 +1239,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
 		}
 
+		/* run terrain estimator */
+		terrain_estimator->predict(dt, &att, &sensor, &lidar);
+		terrain_estimator->measurement_update(hrt_absolute_time(), &gps, &lidar, &att);
+
 		if (inav_verbose_mode) {
 			/* print updates rate */
 			if (t > updates_counter_start + updates_counter_len) {
@@ -1322,6 +1333,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				global_pos.eph = eph;
 				global_pos.epv = epv;
 
+				if (terrain_estimator->is_valid()) {
+					global_pos.terrain_alt = global_pos.alt - terrain_estimator->get_distance_to_ground();
+					global_pos.terrain_alt_valid = true;
+				} else {
+					global_pos.terrain_alt_valid = false;
+				}
+
 				if (vehicle_global_position_pub == NULL) {
 					vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
 

src/modules/px4iofirmware/CMakeLists.txt

@@ -64,6 +64,7 @@ px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags}" GLUE " ")
 include_directories(
 	${include_dirs}
 	${CMAKE_BINARY_DIR}/src/modules/systemlib/mixer
+	.
 	)
 link_directories(${link_dirs})
 add_definitions(${definitions})
@@ -71,11 +72,9 @@ add_definitions(${definitions})
 set(srcs
 	adc.c
 	controls.c
-	dsm.c
 	px4io.c
 	registers.c
 	safety.c
-	sbus.c
 	../systemlib/up_cxxinitialize.c
 	../systemlib/perf_counter.c
 	mixer.cpp
@@ -86,6 +85,8 @@ set(srcs
 	../systemlib/pwm_limit/pwm_limit.c
 	../../lib/rc/st24.c
 	../../lib/rc/sumd.c
+	../../lib/rc/sbus.c
+	../../lib/rc/dsm.c
 	../../drivers/stm32/drv_hrt.c
 	../../drivers/stm32/drv_pwm_servo.c
 	../../drivers/boards/${config_io_board}/px4io_init.c

src/modules/px4iofirmware/controls.c

@@ -46,6 +46,8 @@
 #include <systemlib/ppm_decode.h>
 #include <rc/st24.h>
 #include <rc/sumd.h>
+#include <rc/sbus.h>
+#include <rc/dsm.h>
 
 #include "px4io.h"
 
@@ -60,7 +62,8 @@ static perf_counter_t c_gather_dsm;
 static perf_counter_t c_gather_sbus;
 static perf_counter_t c_gather_ppm;
 
-static int _dsm_fd;
+static int _dsm_fd = -1;
+int _sbus_fd = -1;
 
 static uint16_t rc_value_override = 0;
 
@@ -74,7 +77,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 	uint16_t temp_count = r_raw_rc_count;
 	uint8_t n_bytes = 0;
 	uint8_t *bytes;
-	*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
+	*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS);
 
 	if (*dsm_updated) {
 		r_raw_rc_count = temp_count & 0x7fff;
@@ -158,7 +161,7 @@ controls_init(void)
 	_dsm_fd = dsm_init("/dev/ttyS0");
 
 	/* S.bus input (USART3) */
-	sbus_init("/dev/ttyS2");
+	_sbus_fd = sbus_init("/dev/ttyS2", false);
 
 	/* default to a 1:1 input map, all enabled */
 	for (unsigned i = 0; i < PX4IO_RC_INPUT_CHANNELS; i++) {
@@ -240,7 +243,7 @@ controls_tick()
 	perf_begin(c_gather_sbus);
 
 	bool sbus_failsafe, sbus_frame_drop;
-	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
+	bool sbus_updated = sbus_input(_sbus_fd, r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
 				       PX4IO_RC_INPUT_CHANNELS);
 
 	if (sbus_updated) {

src/modules/px4iofirmware/mixer.cpp

@@ -48,6 +48,7 @@
 
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_hrt.h>
+#include <rc/sbus.h>
 
 #include <systemlib/pwm_limit/pwm_limit.h>
 #include <systemlib/mixer/mixer.h>
@@ -71,6 +72,8 @@ static bool should_arm_nothrottle = false;
 static bool should_always_enable_pwm = false;
 static volatile bool in_mixer = false;
 
+extern int _sbus_fd;
+
 /* selected control values and count for mixing */
 enum mixer_source {
 	MIX_NONE,
@@ -292,10 +295,10 @@ mixer_tick(void)
 		/* set S.BUS1 or S.BUS2 outputs */
 
 		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
-			sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
+			sbus2_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
 
 		} else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
-			sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
+			sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT);
 		}
 
 	} else if (mixer_servos_armed && should_always_enable_pwm) {
@@ -306,11 +309,11 @@ mixer_tick(void)
 
 		/* set S.BUS1 or S.BUS2 outputs */
 		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
-			sbus1_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT);
+			sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT);
 		}
 
 		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
-			sbus2_output(r_page_servo_disarmed, PX4IO_SERVO_COUNT);
+			sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT);
 		}
 	}
 }

src/modules/px4iofirmware/px4io.h

@@ -224,14 +224,6 @@ extern uint16_t	adc_measure(unsigned channel);
  */
 extern void	controls_init(void);
 extern void	controls_tick(void);
-extern int	dsm_init(const char *device);
-extern bool	dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes);
-extern void	dsm_bind(uint16_t cmd, int pulses);
-extern int	sbus_init(const char *device);
-extern bool	sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop,
-			   uint16_t max_channels);
-extern void	sbus1_output(uint16_t *values, uint16_t num_values);
-extern void	sbus2_output(uint16_t *values, uint16_t num_values);
 
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;

src/modules/px4iofirmware/registers.c

@@ -47,6 +47,7 @@
 #include <drivers/drv_pwm_output.h>
 #include <systemlib/systemlib.h>
 #include <stm32_pwr.h>
+#include <rc/dsm.h>
 
 #include "px4io.h"
 #include "protocol.h"

src/modules/sdlog2/sdlog2.c

@@ -311,7 +311,7 @@ int sdlog2_main(int argc, char *argv[])
 		deamon_task = px4_task_spawn_cmd("sdlog2",
 						 SCHED_DEFAULT,
 						 SCHED_PRIORITY_DEFAULT - 30,
-						 3000,
+						 3300,
 						 sdlog2_thread_main,
 						 (char * const *)argv);
 

src/modules/sensors/sensors.cpp

@@ -851,7 +851,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;

src/modules/systemlib/mixer/CMakeLists.txt

@@ -38,7 +38,7 @@ add_custom_command(OUTPUT mixer_multirotor.generated.h
 	> mixer_multirotor.generated.h)
 
 add_custom_target(mixer_gen
-	DEPENDS mixer_multirotor.generated.h)
+	DEPENDS mixer_multirotor.generated.h multi_tables.py)
 
 px4_add_module(
 	MODULE modules__systemlib__mixer
@@ -51,5 +51,6 @@ px4_add_module(
 		mixer_multirotor.generated.h
 	DEPENDS
 		platforms__common
+		mixer_gen
 	)
 # vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -150,6 +150,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	} else if (!strcmp(geomname, "4x")) {
 		geometry = MultirotorGeometry::QUAD_X;
 
+	} else if (!strcmp(geomname, "4h")) {
+		geometry = MultirotorGeometry::QUAD_H;
+
 	} else if (!strcmp(geomname, "4v")) {
 		geometry = MultirotorGeometry::QUAD_V;
 

src/modules/systemlib/mixer/multi_tables.py

@@ -62,6 +62,13 @@ quad_x = [
     [135,  CW],
 ]
 
+quad_h = [
+    [  45, CW],
+    [-135, CW],
+    [-45,  CCW],
+    [135,  CCW],
+]
+
 quad_plus = [
     [  90, CCW],
     [ -90, CCW],
@@ -162,7 +169,7 @@ tri_y = [
 ]
 
 
-tables = [quad_x, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine, tri_y]
+tables = [quad_x, quad_h, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine, tri_y]
 
 def variableName(variable):
     for variableName, value in list(globals().items()):

src/modules/uORB/objects_common.cpp

@@ -164,8 +164,12 @@ ORB_DEFINE(fence_vertex, struct fence_vertex_s);
 
 #include "topics/vehicle_attitude_setpoint.h"
 ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
-ORB_DEFINE(mc_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
-ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
+
+#include "topics/mc_virtual_attitude_setpoint.h"
+ORB_DEFINE(mc_virtual_attitude_setpoint, struct mc_virtual_attitude_setpoint_s);
+
+#include "topics/fw_virtual_attitude_setpoint.h"
+ORB_DEFINE(fw_virtual_attitude_setpoint, struct fw_virtual_attitude_setpoint_s);
 
 #include "topics/manual_control_setpoint.h"
 ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);

src/modules/uORB/uORB.h

@@ -366,7 +366,6 @@ __END_DECLS
 
 /* Diverse uORB header defines */ //XXX: move to better location
 #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS    ORB_ID(actuator_controls_0)
-typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s;
 typedef uint8_t arming_state_t;
 typedef uint8_t main_state_t;
 typedef uint8_t hil_state_t;

src/modules/uavcan/CMakeLists.txt

@@ -41,7 +41,7 @@ add_definitions(
 		-DUAVCAN_NO_ASSERTIONS
 		-DUAVCAN_PLATFORM=stm32
 		-DUAVCAN_STM32_${OS_UPPER}=1
-		-DUAVCAN_STM32_NUM_IFACES=2
+		-DUAVCAN_STM32_NUM_IFACES=${config_uavcan_num_ifaces}
 		-DUAVCAN_STM32_TIMER_NUMBER=5
 		)
 

src/modules/uavcan/uavcan_main.cpp

@@ -533,12 +533,17 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 	 * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to
 	 * fail during initialization.
 	 */
-	#ifdef GPIO_CAN1_RX
+#if defined(GPIO_CAN1_RX)
 	stm32_configgpio(GPIO_CAN1_RX);
 	stm32_configgpio(GPIO_CAN1_TX);
-	#endif
+#endif
+#if defined(GPIO_CAN2_RX)
 	stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP);
 	stm32_configgpio(GPIO_CAN2_TX);
+#endif
+#if !defined(GPIO_CAN1_RX) &&  !defined(GPIO_CAN2_RX)
+# error  "Need to define GPIO_CAN1_RX and/or GPIO_CAN2_RX"
+#endif
 
 	/*
 	 * CAN driver init

src/modules/vtol_att_control/standard.cpp

@@ -186,6 +186,9 @@ void Standard::update_vtol_state()
 
 void Standard::update_transition_state()
 {
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+
 	if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
 		if (_params_standard.front_trans_dur <= 0.0f) {
 			// just set the final target throttle value
@@ -242,11 +245,15 @@ void Standard::update_transition_state()
 
 void Standard::update_mc_state()
 {
-	// do nothing
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 }
 
 void Standard::update_fw_state()
 {
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+
 	// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
 	if (!_flag_enable_mc_motors) {
 		set_max_mc(950);

src/modules/vtol_att_control/tailsitter.cpp

@@ -35,18 +35,42 @@
 * @file tailsitter.cpp
 *
 * @author Roman Bapst 		<bapstroman@gmail.com>
+* @author David Vorsin     <davidvorsin@gmail.com>
 *
 */
 
 #include "tailsitter.h"
 #include "vtol_att_control_main.h"
 
-Tailsitter::Tailsitter(VtolAttitudeControl *att_controller) :
-	VtolType(att_controller),
-	_airspeed_tot(0),
+#define ARSP_YAW_CTRL_DISABLE 7.0f	// airspeed at which we stop controlling yaw during a front transition
+#define THROTTLE_TRANSITION_MAX 0.25f	// maximum added thrust above last value in transition
+#define PITCH_TRANSITION_FRONT_P1 -1.1f	// pitch angle to switch to TRANSITION_P2
+#define PITCH_TRANSITION_FRONT_P2 -1.2f	// pitch angle to switch to FW
+#define PITCH_TRANSITION_BACK -0.25f	// pitch angle to switch to MC
+
+Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
+	VtolType(attc),
+	_airspeed_tot(0.0f),
+	_min_front_trans_dur(0.5f),
+
 	_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
 	_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
 {
+	_vtol_schedule.flight_mode = MC_MODE;
+	_vtol_schedule.transition_start = 0;
+
+	_mc_roll_weight = 1.0f;
+	_mc_pitch_weight = 1.0f;
+	_mc_yaw_weight = 1.0f;
+
+	_flag_was_in_trans_mode = false;
+
+	_params_handles_tailsitter.front_trans_dur = param_find("VT_F_TRANS_DUR");
+	_params_handles_tailsitter.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
+	_params_handles_tailsitter.back_trans_dur = param_find("VT_B_TRANS_DUR");
+	_params_handles_tailsitter.airspeed_trans = param_find("VT_ARSP_TRANS");
+	_params_handles_tailsitter.airspeed_blend_start = param_find("VT_ARSP_BLEND");
+	_params_handles_tailsitter.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
 
 }
 
@@ -55,23 +79,170 @@ Tailsitter::~Tailsitter()
 
 }
 
+int
+Tailsitter::parameters_update()
+{
+	float v;
+	int l;
+
+	/* vtol duration of a front transition */
+	param_get(_params_handles_tailsitter.front_trans_dur, &v);
+	_params_tailsitter.front_trans_dur = math::constrain(v, 1.0f, 5.0f);
+
+	/* vtol front transition phase 2 duration */
+	param_get(_params_handles_tailsitter.front_trans_dur_p2, &v);
+	_params_tailsitter.front_trans_dur_p2 = v;
+
+	/* vtol duration of a back transition */
+	param_get(_params_handles_tailsitter.back_trans_dur, &v);
+	_params_tailsitter.back_trans_dur = math::constrain(v, 0.0f, 5.0f);
+
+	/* vtol airspeed at which it is ok to switch to fw mode */
+	param_get(_params_handles_tailsitter.airspeed_trans, &v);
+	_params_tailsitter.airspeed_trans = v;
+
+	/* vtol airspeed at which we start blending mc/fw controls */
+	param_get(_params_handles_tailsitter.airspeed_blend_start, &v);
+	_params_tailsitter.airspeed_blend_start = v;
+
+	/* vtol lock elevons in multicopter */
+	param_get(_params_handles_tailsitter.elevons_mc_lock, &l);
+	_params_tailsitter.elevons_mc_lock = l;
+
+	/* avoid parameters which will lead to zero division in the transition code */
+	_params_tailsitter.front_trans_dur = math::max(_params_tailsitter.front_trans_dur, _min_front_trans_dur);
+
+	if (_params_tailsitter.airspeed_trans < _params_tailsitter.airspeed_blend_start + 1.0f) {
+		_params_tailsitter.airspeed_trans = _params_tailsitter.airspeed_blend_start + 1.0f;
+	}
+
+	return OK;
+}
+
 void Tailsitter::update_vtol_state()
 {
-	// simply switch between the two modes
-	if (!_attc->is_fixed_wing_requested()) {
-		_vtol_mode = ROTARY_WING;
+	parameters_update();
 
-	} else {
+	/* simple logic using a two way switch to perform transitions.
+	 * after flipping the switch the vehicle will start tilting in MC control mode, picking up
+	 * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode.
+	 * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle.
+	*/
+
+	if (_manual_control_sp->aux1 < 0.0f) {
+
+
+		switch (_vtol_schedule.flight_mode) { // user switchig to MC mode
+		case MC_MODE:
+			break;
+
+		case FW_MODE:
+			_vtol_schedule.flight_mode 	= TRANSITION_BACK;
+			_vtol_schedule.transition_start = hrt_absolute_time();
+			break;
+
+		case TRANSITION_FRONT_P1:
+			// failsafe into multicopter mode
+			_vtol_schedule.flight_mode = MC_MODE;
+			break;
+
+		case TRANSITION_FRONT_P2:
+			// NOT USED
+			// failsafe into multicopter mode
+			//_vtol_schedule.flight_mode = MC_MODE;
+			break;
+
+		case TRANSITION_BACK:
+
+			// check if we have reached pitch angle to switch to MC mode
+			if (_v_att->pitch >= PITCH_TRANSITION_BACK) {
+				_vtol_schedule.flight_mode = MC_MODE;
+			}
+
+			break;
+		}
+
+	} else {  // user switchig to FW mode
+
+		switch (_vtol_schedule.flight_mode) {
+		case MC_MODE:
+			// initialise a front transition
+			_vtol_schedule.flight_mode 	= TRANSITION_FRONT_P1;
+			_vtol_schedule.transition_start = hrt_absolute_time();
+			break;
+
+		case FW_MODE:
+			break;
+
+		case TRANSITION_FRONT_P1:
+
+			// check if we have reached airspeed  and pitch angle to switch to TRANSITION P2 mode
+			if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans
+			    && _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || !_armed->armed) {
+				_vtol_schedule.flight_mode = FW_MODE;
+				//_vtol_schedule.transition_start = hrt_absolute_time();
+			}
+
+			break;
+
+		case TRANSITION_FRONT_P2:
+
+		case TRANSITION_BACK:
+			// failsafe into fixed wing mode
+			_vtol_schedule.flight_mode = FW_MODE;
+
+			/*  **LATER***  if pitch is closer to mc (-45>)
+			*   go to transition P1
+			*/
+			break;
+		}
+	}
+
+	// map tailsitter specific control phases to simple control modes
+	switch (_vtol_schedule.flight_mode) {
+	case MC_MODE:
+		_vtol_mode = ROTARY_WING;
+		_vtol_vehicle_status->vtol_in_trans_mode = false;
+		_flag_was_in_trans_mode = false;
+		break;
+
+	case FW_MODE:
 		_vtol_mode = FIXED_WING;
+		_vtol_vehicle_status->vtol_in_trans_mode = false;
+		_flag_was_in_trans_mode = false;
+		break;
+
+	case TRANSITION_FRONT_P1:
+		_vtol_mode = TRANSITION;
+		_vtol_vehicle_status->vtol_in_trans_mode = true;
+
+	case TRANSITION_FRONT_P2:
+		_vtol_mode = TRANSITION;
+		_vtol_vehicle_status->vtol_in_trans_mode = true;
+
+	case TRANSITION_BACK:
+		_vtol_mode = TRANSITION;
+		_vtol_vehicle_status->vtol_in_trans_mode = true;
+		break;
 	}
 }
 
+
+
 void Tailsitter::update_mc_state()
 {
+	// set idle speed for rotary wing mode
 	if (!flag_idle_mc) {
 		set_idle_mc();
 		flag_idle_mc = true;
 	}
+
+	_mc_roll_weight = 1.0f;
+	_mc_pitch_weight = 1.0f;
+	_mc_yaw_weight = 1.0f;
+
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 }
 
 void Tailsitter::update_fw_state()
@@ -80,13 +251,137 @@ void Tailsitter::update_fw_state()
 		set_idle_fw();
 		flag_idle_mc = false;
 	}
+
+	_mc_roll_weight = 0.0f;
+	_mc_pitch_weight = 0.0f;
+	_mc_yaw_weight = 0.0f;
+
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 }
 
 void Tailsitter::update_transition_state()
 {
+	if (!_flag_was_in_trans_mode) {
+		// save desired heading for transition and last thrust value
+		_yaw_transition = _v_att_sp->yaw_body;
+		_pitch_transition_start = _v_att_sp->pitch_body;
+		_throttle_transition = _v_att_sp->thrust;
+		_flag_was_in_trans_mode = true;
+	}
 
+	if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
+
+		/** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/
+		_v_att_sp->pitch_body = _pitch_transition_start	- (fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) *
+					(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f));
+		_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1 - 0.2f ,
+							_pitch_transition_start);
+
+		/** create time dependant throttle signal higher than  in MC and growing untill  P2 switch speed reached */
+		if (_airspeed->true_airspeed_m_s <= _params_tailsitter.airspeed_trans) {
+			_v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) *
+					    (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f));
+			_v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition ,
+							    (1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition);
+		}
+
+		// disable mc yaw control once the plane has picked up speed
+		if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) {
+			_mc_yaw_weight = 0.0f;
+
+		} else {
+			_mc_yaw_weight = 1.0f;
+		}
+
+		_mc_roll_weight = 1.0f;
+		_mc_pitch_weight = 1.0f;
+
+	} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
+		// the plane is ready to go into fixed wing mode, smoothly switch the actuator controls, keep pitching down
+
+		/** no motor  switching */
+
+		if (flag_idle_mc) {
+			set_idle_fw();
+			flag_idle_mc = false;
+		}
+
+		/** create time dependant pitch angle set point  + 0.2 rad overlap over the switch value*/
+		if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) {
+			_v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 -
+						(fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time(
+							 &_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
+
+			if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) {
+				_v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2 - 0.2f;
+			}
+
+		}
+
+		/** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/
+
+		//_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
+		//_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f));
+
+
+		_mc_roll_weight = 0.0f;
+		_mc_pitch_weight = 0.0f;
+
+		/** keep yaw disabled */
+		_mc_yaw_weight = 0.0f;
+
+
+	} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
+
+		if (!flag_idle_mc) {
+			set_idle_mc();
+			flag_idle_mc = true;
+		}
+
+		/** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/
+		_v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) *
+					(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.back_trans_dur * 1000000.0f);
+		_v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK + 0.2f);
+
+		//  throttle value is decreesed
+		_v_att_sp->thrust = _throttle_transition * 0.9f;
+
+		/** keep yaw disabled */
+		_mc_yaw_weight = 0.0f;
+
+		/** smoothly move control weight to MC */
+		_mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
+				  (_params_tailsitter.back_trans_dur * 1000000.0f);
+		_mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
+				   (_params_tailsitter.back_trans_dur * 1000000.0f);
+
+	}
+
+
+
+
+	_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
+	_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
+	_mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f);
+
+	// compute desired attitude and thrust setpoint for the transition
+
+	_v_att_sp->timestamp = hrt_absolute_time();
+	_v_att_sp->roll_body = 0.0f;
+	_v_att_sp->yaw_body = _yaw_transition;
+	_v_att_sp->R_valid = true;
+
+	math::Matrix<3, 3> R_sp;
+	R_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body);
+	memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body));
+
+	math::Quaternion q_sp;
+	q_sp.from_dcm(R_sp);
+	memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d));
 }
 
+
 void Tailsitter::update_external_state()
 {
 
@@ -110,8 +405,7 @@ void Tailsitter::calc_tot_airspeed()
 	_airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw;
 }
 
-void
-Tailsitter::scale_mc_output()
+void Tailsitter::scale_mc_output()
 {
 	// scale around tuning airspeed
 	float airspeed;
@@ -167,8 +461,7 @@ void Tailsitter::fill_actuator_outputs()
 			// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
 			_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
 				_actuators_mc_in->control[actuator_controls_s::INDEX_YAW];	//roll elevon
-			_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
-				_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];	//pitch elevon
+			_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH];	//pitch elevon
 		}
 
 		break;
@@ -192,6 +485,26 @@ void Tailsitter::fill_actuator_outputs()
 		break;
 
 	case TRANSITION:
+		// in transition engines are mixed by weight (BACK TRANSITION ONLY)
+		_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
+				* _mc_roll_weight;
+		_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
+			_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
+		_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
+				_mc_yaw_weight;
+		_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
+			_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
+
+		// NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation!
+		_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
+				* (1 - _mc_roll_weight);
+		_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
+			_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
+		// **LATER** + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight);
+		_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
+			_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
+		break;
+
 	case EXTERNAL:
 		// not yet implemented, we are switching brute force at the moment
 		break;

src/modules/vtol_att_control/tailsitter.h

@@ -35,6 +35,7 @@
 * @file tiltrotor.h
 *
 * @author Roman Bapst 		<bapstroman@gmail.com>
+* @author David Vorsin     <davidvorsin@gmail.com>
 *
 */
 
@@ -42,7 +43,9 @@
 #define TAILSITTER_H
 
 #include "vtol_type.h"
-#include <systemlib/perf_counter.h>
+#include <systemlib/perf_counter.h>  /** is it necsacery? **/
+#include <systemlib/param/param.h>
+#include <drivers/drv_hrt.h>
 
 class Tailsitter : public VtolType
 {
@@ -51,21 +54,96 @@ public:
 	Tailsitter(VtolAttitudeControl *_att_controller);
 	~Tailsitter();
 
+	/**
+	 * Update vtol state.
+	 */
 	void update_vtol_state();
+
+	/**
+	 * Update multicopter state.
+	 */
 	void update_mc_state();
+
+	/**
+	 * Update fixed wing state.
+	 */
 	void update_fw_state();
+
+	/**
+	 * Update transition state.
+	 */
 	void update_transition_state();
+
+	/**
+	 * Update external state.
+	 */
 	void update_external_state();
 
 private:
-	void fill_actuator_outputs();
-	void calc_tot_airspeed();
-	void scale_mc_output();
 
-	float _airspeed_tot;
 
+
+	struct {
+		float front_trans_dur;			/**< duration of first part of front transition */
+		float front_trans_dur_p2;
+		float back_trans_dur;			/**< duration of back transition */
+		float airspeed_trans;			/**< airspeed at which we switch to fw mode after transition */
+		float airspeed_blend_start;		/**< airspeed at which we start blending mc/fw controls */
+		int elevons_mc_lock;			/**< lock elevons in multicopter mode */
+
+	} _params_tailsitter;
+
+	struct {
+		param_t front_trans_dur;
+		param_t front_trans_dur_p2;
+		param_t back_trans_dur;
+		param_t airspeed_trans;
+		param_t airspeed_blend_start;
+		param_t elevons_mc_lock;
+
+	} _params_handles_tailsitter;
+
+	enum vtol_mode {
+		MC_MODE = 0,			/**< vtol is in multicopter mode */
+		TRANSITION_FRONT_P1,	/**< vtol is in front transition part 1 mode */
+		TRANSITION_FRONT_P2,	/**< vtol is in front transition part 2 mode */
+		TRANSITION_BACK,		/**< vtol is in back transition mode */
+		FW_MODE					/**< vtol is in fixed wing mode */
+	};
+
+	struct {
+		vtol_mode flight_mode;			/**< vtol flight mode, defined by enum vtol_mode */
+		hrt_abstime transition_start;	/**< absoulte time at which front transition started */
+	} _vtol_schedule;
+
+	float _airspeed_tot; 		/** speed estimation for propwash controlled surfaces */
+
+	/** not sure about it yet ?! **/
+	float _min_front_trans_dur;	/**< min possible time in which rotors are rotated into the first position */
+
+
+	/** should this anouncement stay? **/
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
 
+	/**
+	 * Speed estimation for propwash controlled surfaces.
+	 */
+	void calc_tot_airspeed();
+
+
+	/** is this one still needed? */
+	void scale_mc_output();
+
+	/**
+	 * Write control values to actuator output topics.
+	 */
+	void fill_actuator_outputs();
+
+	/**
+	 * Update parameters.
+	 */
+	int parameters_update();
+
 };
 #endif

src/modules/vtol_att_control/tailsitter_params.c

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * 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 tailsitter_params.c
+ * Parameters for vtol attitude controller.
+ *
+ * @author Roman Bapst <bapstroman@gmail.com>
+ * @author David Vorsin     <davidvorsin@gmail.com>
+ */
+
+#include <systemlib/param/param.h>
+
+/**
+ * Duration of front transition phase 2
+ *
+ * Time in seconds it should take for the rotors to rotate forward completely from the point
+ * when the plane has picked up enough airspeed and is ready to go into fixed wind mode.
+ *
+ * @min 0.1
+ * @max 2
+ * @group VTOL Attitude Control
+
+PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/
+
+

src/modules/vtol_att_control/tiltrotor.cpp

@@ -56,6 +56,8 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
 	_mc_pitch_weight = 1.0f;
 	_mc_yaw_weight = 1.0f;
 
+	_flag_was_in_trans_mode = false;
+
 	_params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR");
 	_params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR");
 	_params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC");
@@ -250,6 +252,9 @@ void Tiltrotor::update_vtol_state()
 
 void Tiltrotor::update_mc_state()
 {
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+
 	// make sure motors are not tilted
 	_tilt_control = _params_tiltrotor.tilt_mc;
 
@@ -271,6 +276,9 @@ void Tiltrotor::update_mc_state()
 
 void Tiltrotor::update_fw_state()
 {
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+
 	// make sure motors are tilted forward
 	_tilt_control = _params_tiltrotor.tilt_fw;
 
@@ -292,6 +300,13 @@ void Tiltrotor::update_fw_state()
 
 void Tiltrotor::update_transition_state()
 {
+	if (!_flag_was_in_trans_mode) {
+		// save desired heading for transition and last thrust value
+		_yaw_transition = _v_att->yaw;
+		_throttle_transition = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE];
+		_flag_was_in_trans_mode = true;
+	}
+
 	if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
 		// for the first part of the transition the rear rotors are enabled
 		if (_rear_motors != ENABLED) {
@@ -354,6 +369,9 @@ void Tiltrotor::update_transition_state()
 
 	_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
 	_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
+
+	// copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp)
+	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 }
 
 void Tiltrotor::update_external_state()

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -41,6 +41,7 @@
  * @author Roman Bapst 		<bapstr@ethz.ch>
  * @author Lorenz Meier 	<lm@inf.ethz.ch>
  * @author Thomas Gubler	<thomasgubler@gmail.com>
+ * @author David Vorsin     <davidvorsin@gmail.com>
  *
  */
 #include "vtol_att_control_main.h"
@@ -60,6 +61,8 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	//init subscription handlers
 	_v_att_sub(-1),
 	_v_att_sp_sub(-1),
+	_mc_virtual_att_sp_sub(-1),
+	_fw_virtual_att_sp_sub(-1),
 	_mc_virtual_v_rates_sp_sub(-1),
 	_fw_virtual_v_rates_sp_sub(-1),
 	_v_control_mode_sub(-1),
@@ -75,13 +78,16 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_actuators_0_pub(nullptr),
 	_actuators_1_pub(nullptr),
 	_vtol_vehicle_status_pub(nullptr),
-	_v_rates_sp_pub(nullptr)
+	_v_rates_sp_pub(nullptr),
+	_v_att_sp_pub(nullptr)
 
 {
 	memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
 	_vtol_vehicle_status.vtol_in_rw_mode = true;	/* start vtol in rotary wing mode*/
 	memset(&_v_att, 0, sizeof(_v_att));
 	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	memset(&_mc_virtual_att_sp, 0, sizeof(_mc_virtual_att_sp));
+	memset(&_fw_virtual_att_sp, 0, sizeof(_fw_virtual_att_sp));
 	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
 	memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp));
 	memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp));
@@ -271,6 +277,36 @@ VtolAttitudeControl::vehicle_airspeed_poll()
 	}
 }
 
+/**
+* Check for attitude set points update.
+*/
+void
+VtolAttitudeControl::vehicle_attitude_setpoint_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_v_att_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
+	}
+}
+
+/**
+* Check for attitude update.
+*/
+void
+VtolAttitudeControl::vehicle_attitude_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_v_att_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
+	}
+}
+
 /**
 * Check for battery updates.
 */
@@ -319,6 +355,38 @@ VtolAttitudeControl::vehicle_local_pos_poll()
 
 }
 
+/**
+* Check for mc virtual attitude setpoint updates.
+*/
+void
+VtolAttitudeControl::mc_virtual_att_sp_poll()
+{
+	bool updated;
+
+	orb_check(_mc_virtual_att_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub , &_mc_virtual_att_sp);
+	}
+
+}
+
+/**
+* Check for fw virtual attitude setpoint updates.
+*/
+void
+VtolAttitudeControl::fw_virtual_att_sp_poll()
+{
+	bool updated;
+
+	orb_check(_fw_virtual_att_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(fw_virtual_attitude_setpoint), _fw_virtual_att_sp_sub , &_fw_virtual_att_sp);
+	}
+
+}
+
 /**
 * Check for command updates.
 */
@@ -439,6 +507,18 @@ void VtolAttitudeControl::fill_fw_att_rates_sp()
 	_v_rates_sp.thrust 	= _fw_virtual_v_rates_sp.thrust;
 }
 
+void VtolAttitudeControl::publish_att_sp()
+{
+	if (_v_att_sp_pub != nullptr) {
+		/* publish the attitude setpoint */
+		orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp);
+
+	} else {
+		/* advertise and publish */
+		_v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
+	}
+}
+
 void
 VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
 {
@@ -452,9 +532,12 @@ void VtolAttitudeControl::task_main()
 
 	/* do subscriptions */
 	_v_att_sp_sub          = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));
+	_fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint));
 	_mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));
 	_fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));
 	_v_att_sub             = orb_subscribe(ORB_ID(vehicle_attitude));
+	_v_att_sp_sub          = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	_v_control_mode_sub    = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub            = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -523,9 +606,13 @@ void VtolAttitudeControl::task_main()
 
 		_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
 
+		mc_virtual_att_sp_poll();
+		fw_virtual_att_sp_poll();
 		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
 		vehicle_manual_poll();			//Check for changes in manual inputs.
 		arming_status_poll();			//Check for arming status updates.
+		vehicle_attitude_setpoint_poll();//Check for changes in attitude set points
+		vehicle_attitude_poll();		//Check for changes in attitude
 		actuator_controls_mc_poll();	//Check for changes in mc_attitude_control output
 		actuator_controls_fw_poll();	//Check for changes in fw_attitude_control output
 		vehicle_rates_sp_mc_poll();
@@ -582,6 +669,7 @@ void VtolAttitudeControl::task_main()
 		} else if (_vtol_type->get_mode() == TRANSITION) {
 			// vehicle is doing a transition
 			_vtol_vehicle_status.vtol_in_trans_mode = true;
+			_vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition
 
 			bool got_new_data = false;
 
@@ -599,6 +687,7 @@ void VtolAttitudeControl::task_main()
 			if (got_new_data) {
 				_vtol_type->update_transition_state();
 				fill_mc_att_rates_sp();
+				publish_att_sp();
 			}
 
 		} else if (_vtol_type->get_mode() == EXTERNAL) {
@@ -606,6 +695,7 @@ void VtolAttitudeControl::task_main()
 			_vtol_type->update_external_state();
 		}
 
+		publish_att_sp();
 		_vtol_type->fill_actuator_outputs();
 
 		/* Only publish if the proper mode(s) are enabled */