Merge branch 'master' into ekf_voting

.travis.yml

@@ -13,6 +13,7 @@ cache:
 addons:
   apt:
     sources:
+    - kubuntu-backports
     - ubuntu-toolchain-r-test
     packages:
     - build-essential
@@ -46,6 +47,12 @@ before_script:
   - mkdir -p ~/bin
   - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++
   - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc
+  - ln -s /usr/bin/ccache ~/bin/clang++
+  - ln -s /usr/bin/ccache ~/bin/clang++-3.4
+  - ln -s /usr/bin/ccache ~/bin/clang++-3.5
+  - ln -s /usr/bin/ccache ~/bin/clang
+  - ln -s /usr/bin/ccache ~/bin/clang-3.4
+  - ln -s /usr/bin/ccache ~/bin/clang-3.5
   - ln -s /usr/bin/ccache ~/bin/g++-4.8
   - ln -s /usr/bin/ccache ~/bin/gcc-4.8
   - export PATH=~/bin:$PATH

Makefile

@@ -220,7 +220,7 @@ qurtrun:
 # unit tests.
 .PHONY: tests
 tests:	generateuorbtopicheaders
-	$(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) unittests)
+	$(Q) (mkdir -p $(PX4_BASE)/unittests/build && cd $(PX4_BASE)/unittests/build && cmake .. && $(MAKE) --no-print-directory unittests)
 
 .PHONY: format check_format
 check_format:

ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil

@@ -39,8 +39,5 @@ then
 	param set FW_RR_P 0.3
 fi
 
-# Enable gamepad / joystick support
-param set COM_RC_IN_MODE 2
-
 set HIL yes
 set MIXER AERT

ROMFS/px4fmu_common/init.d/10016_3dr_iris

@@ -13,11 +13,11 @@ if [ $AUTOCNF == yes ]
 then
 	# TODO tune roll/pitch separately
 	param set MC_ROLL_P 7.0
-	param set MC_ROLLRATE_P 0.13
+	param set MC_ROLLRATE_P 0.15
 	param set MC_ROLLRATE_I 0.05
 	param set MC_ROLLRATE_D 0.004
 	param set MC_PITCH_P 7.0
-	param set MC_PITCHRATE_P 0.13
+	param set MC_PITCHRATE_P 0.15
 	param set MC_PITCHRATE_I 0.05
 	param set MC_PITCHRATE_D 0.004
 	param set MC_YAW_P 2.5

ROMFS/px4fmu_common/init.d/1001_rc_quad_x.hil

@@ -11,7 +11,4 @@ sh /etc/init.d/rc.mc_defaults
 
 set MIXER quad_x
 
-# Enable gamepad / joystick support
-param set COM_RC_IN_MODE 2
-
 set HIL yes

ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil

@@ -11,7 +11,4 @@ sh /etc/init.d/rc.mc_defaults
 
 set MIXER quad_+
 
-# Enable gamepad / joystick support
-param set COM_RC_IN_MODE 2
-
 set HIL yes

ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil

@@ -11,7 +11,4 @@ sh /etc/init.d/rc.fw_defaults
 
 set HIL yes
 
-# Enable gamepad / joystick support
-param set COM_RC_IN_MODE 2
-
 set MIXER AERT

ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil

@@ -36,8 +36,5 @@ fi
 
 set HIL yes
 
-# Enable gamepad / joystick support
-param set COM_RC_IN_MODE 2
-
 # Set the AERT mixer for HIL (even if the malolo is a flying wing)
 set MIXER AERT

ROMFS/px4fmu_common/init.d/4010_dji_f330

@@ -16,11 +16,11 @@ sh /etc/init.d/4001_quad_x
 if [ $AUTOCNF == yes ]
 then
 	param set MC_ROLL_P 7.0
-	param set MC_ROLLRATE_P 0.13
+	param set MC_ROLLRATE_P 0.15
 	param set MC_ROLLRATE_I 0.05
 	param set MC_ROLLRATE_D 0.003
 	param set MC_PITCH_P 7.0
-	param set MC_PITCHRATE_P 0.13
+	param set MC_PITCHRATE_P 0.15
 	param set MC_PITCHRATE_I 0.05
 	param set MC_PITCHRATE_D 0.003
 	param set MC_YAW_P 2.8

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -16,8 +16,6 @@ then
 	param set PE_VELD_NOISE 0.35
 	param set PE_POSNE_NOISE 0.5
 	param set PE_POSD_NOISE 1.0
-	param set PE_GBIAS_PNOISE 0.000001
-	param set PE_ABIAS_PNOISE 0.0002
 fi
 
 # This is the gimbal pass mixer

ROMFS/px4fmu_common/init.d/rc.mc_defaults

@@ -8,8 +8,6 @@ then
 	param set PE_VELD_NOISE 0.35
 	param set PE_POSNE_NOISE 0.5
 	param set PE_POSD_NOISE 1.25
-	param set PE_GBIAS_PNOISE 0.000001
-	param set PE_ABIAS_PNOISE 0.0001
 
 	param set NAV_ACC_RAD 2.0
 	param set RTL_RETURN_ALT 30.0

ROMFS/px4fmu_common/init.d/rc.vtol_defaults

@@ -37,7 +37,6 @@ then
 	param set PE_VELD_NOISE 0.3
 	param set PE_POSNE_NOISE 0.5
 	param set PE_POSD_NOISE 1.25
-	param set PE_GBIAS_PNOISE 0.000001
 	param set PE_ABIAS_PNOISE 0.0001
 fi
 

makefiles/firmware.mk

@@ -106,8 +106,6 @@ ifneq ($(words $(PX4_BASE)),1)
 $(error Cannot build when the PX4_BASE path contains one or more space characters.)
 endif
 
-$(info %  GIT_DESC            = $(GIT_DESC))
-
 #
 # Set a default target so that included makefiles or errors here don't
 # cause confusion.
@@ -232,7 +230,7 @@ $(MODULE_OBJS):		workdir = $(@D)
 $(MODULE_OBJS):		$(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER)
 	$(Q) $(MKDIR) -p $(workdir)
 	$(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \
-		-C $(workdir) \
+		--no-print-directory -C $(workdir) \
 		MODULE_WORK_DIR=$(workdir) \
 		MODULE_OBJ=$@ \
 		MODULE_MK=$(mkfile) \
@@ -292,7 +290,7 @@ $(LIBRARY_LIBS):	workdir = $(@D)
 $(LIBRARY_LIBS):	$(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER)
 	$(Q) $(MKDIR) -p $(workdir)
 	$(Q) $(MAKE) -r -f $(PX4_MK_DIR)library.mk \
-		-C $(workdir) \
+		--no-print-directory -C $(workdir) \
 		LIBRARY_WORK_DIR=$(workdir) \
 		LIBRARY_LIB=$@ \
 		LIBRARY_MK=$(mkfile) \

makefiles/module.mk

@@ -113,7 +113,9 @@
 ifeq ($(MODULE_MK),)
 $(error No module makefile specified)
 endif
+ifeq ($(V),1)
 $(info %% MODULE_MK           = $(MODULE_MK))
+endif
 
 #
 # Get the board/toolchain config
@@ -125,10 +127,12 @@ include $(BOARD_FILE)
 #
 include $(MODULE_MK)
 MODULE_SRC		:= $(dir $(MODULE_MK))
+ifeq ($(V),1)
 $(info %  MODULE_NAME         = $(MODULE_NAME))
 $(info %  MODULE_SRC          = $(MODULE_SRC))
 $(info %  MODULE_OBJ          = $(MODULE_OBJ))
 $(info %  MODULE_WORK_DIR     = $(MODULE_WORK_DIR))
+endif
 
 #
 # Things that, if they change, might affect everything

makefiles/nuttx/firmware_nuttx.mk

@@ -30,7 +30,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.px4:	checkgitversion generateuorbtopi
 	@$(ECHO) %%%% Building $(config) in $(work_dir)
 	@$(ECHO) %%%%
 	$(Q) $(MKDIR) -p $(work_dir)
-	$(Q) $(MAKE) -r -C $(work_dir) \
+	$(Q) $(MAKE) -r --no-print-directory -C $(work_dir) \
 		-f $(PX4_MK_DIR)firmware.mk \
 		CONFIG=$(config) \
 		WORK_DIR=$(work_dir) \
@@ -77,11 +77,11 @@ $(ARCHIVE_DIR)%.export:	configuration = nsh
 $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC)
 	@$(ECHO) %% Configuring NuttX for $(board)
 	$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
-	$(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) distclean
 	$(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(board) .)
 	$(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(board)/$(configuration))
 	@$(ECHO) %% Exporting NuttX for $(board)
-	$(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export
+	$(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) CONFIG_ARCH_BOARD=$(board) export
 	$(Q) $(MKDIR) -p $(dir $@)
 	$(Q) $(COPY) $(NUTTX_SRC)nuttx-export.zip $@
 	$(Q) (cd $(NUTTX_SRC)/configs && $(RMDIR) $(board))
@@ -98,12 +98,12 @@ BOARD			 = $(BOARDS)
 menuconfig: $(NUTTX_SRC)
 	@$(ECHO) %% Configuring NuttX for $(BOARD)
 	$(Q) (cd $(NUTTX_SRC) && $(RMDIR) nuttx-export)
-	$(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) distclean
 	$(Q) (cd $(NUTTX_SRC)/configs && $(COPYDIR) $(PX4_BASE)nuttx-configs/$(BOARD) .)
 	$(Q) (cd $(NUTTX_SRC)tools && ./configure.sh $(BOARD)/nsh)
 	@$(ECHO) %% Running menuconfig for $(BOARD)
-	$(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) oldconfig
+	$(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) oldconfig
-	$(Q) $(MAKE) -r -j$(J) -C $(NUTTX_SRC) -r $(MQUIET) menuconfig
+	$(Q) $(MAKE) -r -j$(J) --no-print-directory -C $(NUTTX_SRC) -r $(MQUIET) menuconfig
 	@$(ECHO) %% Saving configuration file
 	$(Q)$(COPY) $(NUTTX_SRC).config $(PX4_BASE)nuttx-configs/$(BOARD)/nsh/defconfig
 else

makefiles/nuttx/toolchain_gnu-arm-eabi.mk

@@ -147,7 +147,6 @@ ARCHWARNINGS		 = -Wall \
 			   -Wshadow \
 			   -Wfloat-equal \
 			   -Wpointer-arith \
-			   -Wlogical-op \
 			   -Wmissing-declarations \
 			   -Wpacked \
 			   -Wno-unused-parameter \
@@ -271,7 +270,6 @@ define PRELINK
 	@$(ECHO) "PRELINK: $1"
 	@$(MKDIR) -p $(dir $1)
 	$(Q) $(LD) -Ur -Map $1.map  -o $1 $2 && $(OBJCOPY) --localize-hidden $1
-	#$(Q) $(LD) -Ur -Map $1.map  -o $1 $2 && $(OBJCOPY) --localize-hidden $1
 endef
 
 # Update the archive $1 with the files in $2

makefiles/posix-arm/toolchain_gnu-arm-linux-gnueabihf.mk

@@ -157,8 +157,6 @@ ARCHWARNINGS		 = -Wall \
 			   -Werror=reorder \
 			   -Werror=uninitialized \
 			   -Werror=init-self \
-			   -Wno-error=logical-op \
-			   -Wlogical-op \
 			   -Wformat=1 \
 			   -Werror=unused-but-set-variable \
 			   -Wno-error=double-promotion \

makefiles/posix/firmware_posix.mk

@@ -47,7 +47,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.a:	checkgitversion generateuorbtopich
 	@$(ECHO) %%%% Building $(config) in $(work_dir)
 	@$(ECHO) %%%%
 	$(Q) $(MKDIR) -p $(work_dir)
-	$(Q) $(MAKE) -r -C $(work_dir) \
+	$(Q) $(MAKE) -r --no-print-directory -C $(work_dir) \
 		-f $(PX4_MK_DIR)firmware.mk \
 		CONFIG=$(config) \
 		WORK_DIR=$(work_dir) \

makefiles/posix/toolchain_native.mk

@@ -183,9 +183,8 @@ ARCHWARNINGS		 = -Wall \
 
 # Add compiler specific options
 ifeq ($(USE_GCC),1)
-ARCHDEFINES		+= -Wno-error=logical-op
+ARCHDEFINES		+=
 ARCHWARNINGS		+= -Wdouble-promotion \
-			   -Wlogical-op \
 			   -Wformat=1 \
 			   -Werror=unused-but-set-variable \
 			   -Werror=double-promotion
@@ -348,7 +347,6 @@ endef
 define LINK_A
 	@$(ECHO) "LINK_A:    $1"
 	@$(MKDIR) -p $(dir $1)
-	echo "$(Q) $(AR) $1 $2"
 	$(Q) $(AR) $1 $2
 endef
 
@@ -357,7 +355,6 @@ endef
 define LINK_SO
 	@$(ECHO) "LINK_SO:    $1"
 	@$(MKDIR) -p $(dir $1)
-	echo "$(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) $(EXTRA_LIBS)"
 	$(Q) $(CXX) $(LDFLAGS) -shared -Wl,-soname,`basename $1`.1 -o $1 $2 $(LIBS) -pthread -lc
 endef
 

makefiles/qurt/firmware_qurt.mk

@@ -47,7 +47,7 @@ $(FIRMWARES): $(BUILD_DIR)%.build/firmware.a:	generateuorbtopicheaders
 	@$(ECHO) %%%% Building $(config) in $(work_dir)
 	@$(ECHO) %%%%
 	$(Q) $(MKDIR) -p $(work_dir)
-	$(Q) $(MAKE) -r -C $(work_dir) \
+	$(Q) $(MAKE) -r --no-print-directory -C $(work_dir) \
 		-f $(PX4_MK_DIR)firmware.mk \
 		CONFIG=$(config) \
 		WORK_DIR=$(work_dir) \

makefiles/qurt/toolchain_hexagon.mk

@@ -47,7 +47,7 @@ endif
 #
 HEXAGON_TOOLS_ROOT	 ?= /opt/6.4.03
 #HEXAGON_TOOLS_ROOT	 = /opt/6.4.05
-HEXAGON_SDK_ROOT	 = /opt/Hexagon_SDK/2.0
+HEXAGON_SDK_ROOT	 ?= /opt/Hexagon_SDK/2.0
 V_ARCH			 = v5
 CROSSDEV		 = hexagon-
 HEXAGON_BIN		 = $(addsuffix /gnu/bin,$(HEXAGON_TOOLS_ROOT))

posix-configs/SITL/init/rc.fixed_wing

@@ -4,7 +4,6 @@ param load
 param set MAV_TYPE 1
 param set SYS_AUTOSTART 3033
 param set SYS_RESTART_TYPE 2
-param set COM_RC_IN_MODE 2
 dataman start
 param set CAL_GYRO0_ID 2293760
 param set CAL_ACC0_ID 1376256
@@ -29,18 +28,20 @@ accelsim start
 barosim start
 adcsim start
 gpssim start
+measairspeedsim start
 pwm_out_sim mode_pwm
-commander start
 sensors start
+commander start
 land_detector start fixedwing
 navigator start
 ekf_att_pos_estimator start
 fw_att_control start
 fw_pos_control_l1 start
-mixer load /dev/pwm_output0 ../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
+mixer load /dev/pwm_output0 ../../ROMFS/px4fmu_common/mixers/IO_pass.main.mix
 mavlink start -u 14556 -r 60000
 mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
 mavlink stream -r 50 -s ATTITUDE -u 14556
 mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
 mavlink boot_complete
+sdlog2 start -r 100 -e -t -a

posix-configs/SITL/init/rcS

@@ -8,7 +8,6 @@ param set MC_YAW_P 2.0
 param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
-param set COM_RC_IN_MODE 2
 dataman start
 param set CAL_GYRO0_ID 2293760
 param set CAL_ACC0_ID 1376256
@@ -35,8 +34,8 @@ barosim start
 adcsim start
 gpssim start
 pwm_out_sim mode_pwm
-commander start
 sensors start
+commander start
 land_detector start multicopter
 navigator start
 attitude_estimator_q start
@@ -53,3 +52,4 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556
 mavlink stream -r 20 -s RC_CHANNELS -u 14556
 mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink boot_complete
+sdlog2 start -r 100 -e -t -a

posix-configs/SITL/init/rc_iris_ros

@@ -8,7 +8,6 @@ param set MC_YAW_P 2.0
 param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
-param set COM_RC_IN_MODE 2
 dataman start
 param set CAL_GYRO0_ID 2293760
 param set CAL_ACC0_ID 1376256
@@ -49,8 +48,8 @@ barosim start
 adcsim start
 gpssim start
 pwm_out_sim mode_pwm
-commander start
 sensors start
+commander start
 land_detector start multicopter
 navigator start
 attitude_estimator_q start

src/drivers/device/vdev_posix.cpp

@@ -270,7 +270,7 @@ int px4_fsync(int fd)
 
 int px4_access(const char *pathname, int mode)
 {
-	if (mode == F_OK) {
+	if (mode != F_OK) {
 		errno = EINVAL;
 		return -1;
 	}

src/drivers/stm32/tone_alarm/tone_alarm.cpp

@@ -117,8 +117,23 @@
 
 #include <systemlib/err.h>
 
+/* Check that tone alarm and HRT timers are different */
+#if defined(TONE_ALARM_TIMER)  && defined(HRT_TIMER)
+# if TONE_ALARM_TIMER == HRT_TIMER
+#   error TONE_ALARM_TIMER and HRT_TIMER must use different timers.
+# endif
+#endif
+
 /* Tone alarm configuration */
-#if   TONE_ALARM_TIMER == 2
+#if   TONE_ALARM_TIMER == 1
+# define TONE_ALARM_BASE              STM32_TIM1_BASE
+# define TONE_ALARM_CLOCK             STM32_APB2_TIM1_CLKIN
+# define TONE_ALARM_CLOCK_POWER_REG   STM32_RCC_APB2ENR
+# define TONE_ALARM_CLOCK_ENABLE      RCC_APB2ENR_TIM1EN
+# ifdef CONFIG_STM32_TIM1
+#  error Must not set CONFIG_STM32_TIM1 when TONE_ALARM_TIMER is 1
+# endif
+#elif TONE_ALARM_TIMER == 2
 # define TONE_ALARM_BASE		STM32_TIM2_BASE
 # define TONE_ALARM_CLOCK		STM32_APB1_TIM2_CLKIN
 # define TONE_ALARM_CLOCK_POWER_REG   STM32_RCC_APB1ENR
@@ -150,6 +165,14 @@
 # ifdef CONFIG_STM32_TIM5
 #  error Must not set CONFIG_STM32_TIM5 when TONE_ALARM_TIMER is 5
 # endif
+#elif TONE_ALARM_TIMER == 8
+# define TONE_ALARM_BASE		STM32_TIM8_BASE
+# define TONE_ALARM_CLOCK		STM32_APB2_TIM8_CLKIN
+# define TONE_ALARM_CLOCK_POWER_REG   STM32_RCC_APB2ENR
+# define TONE_ALARM_CLOCK_ENABLE	RCC_APB2ENR_TIM8EN
+# ifdef CONFIG_STM32_TIM8
+#  error Must not set CONFIG_STM32_TIM8 when TONE_ALARM_TIMER is 8
+# endif
 #elif TONE_ALARM_TIMER == 9
 # define TONE_ALARM_BASE		STM32_TIM9_BASE
 # define TONE_ALARM_CLOCK		STM32_APB2_TIM9_CLKIN
@@ -175,7 +198,7 @@
 #  error Must not set CONFIG_STM32_TIM11 when TONE_ALARM_TIMER is 11
 # endif
 #else
-# error Must set TONE_ALARM_TIMER to a generic timer in order to use this driver.
+# error Must set TONE_ALARM_TIMER to one of the timers between 1 and 11 (inclusive) to use this driver.
 #endif
 
 #if TONE_ALARM_CHANNEL == 1
@@ -208,24 +231,47 @@
  */
 #define REG(_reg)	(*(volatile uint32_t *)(TONE_ALARM_BASE + _reg))
 
-#define rCR1     	REG(STM32_GTIM_CR1_OFFSET)
+#if TONE_ALARM_TIMER == 1 || TONE_ALARM_TIMER == 8 // Note: If using TIM1 or TIM8, then you are using the ADVANCED timers and NOT the GENERAL TIMERS, therefore different registers
-#define rCR2     	REG(STM32_GTIM_CR2_OFFSET)
+# define rCR1         REG(STM32_ATIM_CR1_OFFSET)
-#define rSMCR    	REG(STM32_GTIM_SMCR_OFFSET)
+# define rCR2         REG(STM32_ATIM_CR2_OFFSET)
-#define rDIER    	REG(STM32_GTIM_DIER_OFFSET)
+# define rSMCR        REG(STM32_ATIM_SMCR_OFFSET)
-#define rSR      	REG(STM32_GTIM_SR_OFFSET)
+# define rDIER        REG(STM32_ATIM_DIER_OFFSET)
-#define rEGR     	REG(STM32_GTIM_EGR_OFFSET)
+# define rSR          REG(STM32_ATIM_SR_OFFSET)
-#define rCCMR1   	REG(STM32_GTIM_CCMR1_OFFSET)
+# define rEGR         REG(STM32_ATIM_EGR_OFFSET)
-#define rCCMR2   	REG(STM32_GTIM_CCMR2_OFFSET)
+# define rCCMR1       REG(STM32_ATIM_CCMR1_OFFSET)
-#define rCCER    	REG(STM32_GTIM_CCER_OFFSET)
+# define rCCMR2       REG(STM32_ATIM_CCMR2_OFFSET)
-#define rCNT     	REG(STM32_GTIM_CNT_OFFSET)
+# define rCCER        REG(STM32_ATIM_CCER_OFFSET)
-#define rPSC     	REG(STM32_GTIM_PSC_OFFSET)
+# define rCNT         REG(STM32_ATIM_CNT_OFFSET)
-#define rARR     	REG(STM32_GTIM_ARR_OFFSET)
+# define rPSC         REG(STM32_ATIM_PSC_OFFSET)
-#define rCCR1    	REG(STM32_GTIM_CCR1_OFFSET)
+# define rARR         REG(STM32_ATIM_ARR_OFFSET)
-#define rCCR2    	REG(STM32_GTIM_CCR2_OFFSET)
+# define rRCR         REG(STM32_ATIM_RCR_OFFSET)
-#define rCCR3    	REG(STM32_GTIM_CCR3_OFFSET)
+# define rCCR1        REG(STM32_ATIM_CCR1_OFFSET)
-#define rCCR4    	REG(STM32_GTIM_CCR4_OFFSET)
+# define rCCR2        REG(STM32_ATIM_CCR2_OFFSET)
-#define rDCR     	REG(STM32_GTIM_DCR_OFFSET)
+# define rCCR3        REG(STM32_ATIM_CCR3_OFFSET)
-#define rDMAR    	REG(STM32_GTIM_DMAR_OFFSET)
+# define rCCR4        REG(STM32_ATIM_CCR4_OFFSET)
+# define rBDTR        REG(STM32_ATIM_BDTR_OFFSET)
+# define rDCR         REG(STM32_ATIM_DCR_OFFSET)
+# define rDMAR        REG(STM32_ATIM_DMAR_OFFSET)
+#else
+# define rCR1     	REG(STM32_GTIM_CR1_OFFSET)
+# define rCR2     	REG(STM32_GTIM_CR2_OFFSET)
+# define rSMCR    	REG(STM32_GTIM_SMCR_OFFSET)
+# define rDIER    	REG(STM32_GTIM_DIER_OFFSET)
+# define rSR      	REG(STM32_GTIM_SR_OFFSET)
+# define rEGR     	REG(STM32_GTIM_EGR_OFFSET)
+# define rCCMR1   	REG(STM32_GTIM_CCMR1_OFFSET)
+# define rCCMR2   	REG(STM32_GTIM_CCMR2_OFFSET)
+# define rCCER    	REG(STM32_GTIM_CCER_OFFSET)
+# define rCNT     	REG(STM32_GTIM_CNT_OFFSET)
+# define rPSC     	REG(STM32_GTIM_PSC_OFFSET)
+# define rARR     	REG(STM32_GTIM_ARR_OFFSET)
+# define rCCR1    	REG(STM32_GTIM_CCR1_OFFSET)
+# define rCCR2    	REG(STM32_GTIM_CCR2_OFFSET)
+# define rCCR3    	REG(STM32_GTIM_CCR3_OFFSET)
+# define rCCR4    	REG(STM32_GTIM_CCR4_OFFSET)
+# define rDCR     	REG(STM32_GTIM_DCR_OFFSET)
+# define rDMAR    	REG(STM32_GTIM_DMAR_OFFSET)
+#endif
 
 class ToneAlarm : public device::CDev
 {
@@ -396,6 +442,10 @@ ToneAlarm::init()
 	rCCER = TONE_CCER;
 	rDCR = 0;
 
+#ifdef rBDTR // If using an advanced timer, you need to activate the output
+   rBDTR = ATIM_BDTR_MOE; // enable the main output of the advanced timer
+#endif
+
 	/* toggle the CC output each time the count passes 1 */
 	TONE_rCCR = 1;
 

src/modules/commander/commander.cpp

@@ -1220,13 +1220,17 @@ int commander_thread_main(int argc, char *argv[])
 	}
 
 	// Run preflight check
+	int32_t rc_in_off = 0;
 	param_get(_param_autostart_id, &autostart_id);
 	if (is_hil_setup(autostart_id)) {
 		// HIL configuration selected: real sensors will be disabled
 		status.condition_system_sensors_initialized = false;
 		set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
 	} else {
-		status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check);
+		param_get(_param_rc_in_off, &rc_in_off);
+			status.rc_input_mode = rc_in_off;
+		status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true,
+			checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check);
 		if (!status.condition_system_sensors_initialized) {
 			set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
 		}
@@ -1242,7 +1246,6 @@ int commander_thread_main(int argc, char *argv[])
 	int32_t datalink_loss_enabled = false;
 	int32_t datalink_loss_timeout = 10;
 	float rc_loss_timeout = 0.5;
-	int32_t rc_in_off = 0;
 	int32_t datalink_regain_timeout = 0;
 
 	/* Thresholds for engine failure detection */
@@ -1863,17 +1866,16 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* RC input check */
-		if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 &&
+		if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
-		    hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
+		    (hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f))) {
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
 				status.rc_signal_found_once = true;
-				mavlink_log_info(mavlink_fd, "Detected radio control");
 				status_changed = true;
 
 			} else {
 				if (status.rc_signal_lost) {
-					mavlink_log_info(mavlink_fd, "RC SIGNAL REGAINED after %llums",
+					mavlink_log_info(mavlink_fd, "MANUAL CONTROL REGAINED after %llums",
 							     (hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000);
 					status_changed = true;
 				}
@@ -1913,8 +1915,7 @@ int commander_thread_main(int argc, char *argv[])
 			}
 
 			/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
-			if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY &&
+			if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
-			    sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 
 					/* we check outside of the transition function here because the requirement
@@ -1925,13 +1926,15 @@ int commander_thread_main(int argc, char *argv[])
 						(status.main_state != vehicle_status_s::MAIN_STATE_STAB)) {
 						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 
-					} else {
+					} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
 						arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
 										     mavlink_fd);
 
 						if (arming_ret == TRANSITION_CHANGED) {
 							arming_state_changed = true;
 						}
+					} else {
+						print_reject_arm("NOT ARMING: Configuration error");
 					}
 
 					stick_on_counter = 0;
@@ -1978,8 +1981,8 @@ int commander_thread_main(int argc, char *argv[])
 			}
 
 		} else {
-			if (!status.rc_signal_lost) {
+			if (!status.rc_input_blocked && !status.rc_signal_lost) {
-				mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)", hrt_absolute_time() / 1000);
+				mavlink_log_critical(mavlink_fd, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000);
 				status.rc_signal_lost = true;
 				status.rc_signal_lost_timestamp = sp_man.timestamp;
 				status_changed = true;

src/modules/commander/state_machine_helper.cpp

@@ -245,9 +245,7 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
 			(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
 			(!status->condition_system_sensors_initialized)) {
-			if (!fRunPreArmChecks) {
+			mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
-				mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
-			}
 			feedback_provided = true;
 			valid_transition = false;
 			status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c

@@ -214,23 +214,23 @@ PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f);
  * Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f.
  * Increasing this value will make the gyro bias converge faster but noisier.
  *
- * @min 0.0000001
+ * @min 0.00000005
  * @max 0.00001
  * @group Position Estimator
  */
-PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-06f);
+PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
 
 /**
  * Accelerometer bias estimate process noise
  *
- * Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f.
+ * Generic defaults: 0.00001f, multicopters: 0.00001f, ground vehicles: 0.00001f.
  * Increasing this value makes the bias estimation faster and noisier.
  *
  * @min 0.00001
  * @max 0.001
  * @group Position Estimator
  */
-PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0002f);
+PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 1e-05f);
 
 /**
  * Magnetometer earth frame offsets process noise

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -648,8 +648,8 @@ FixedwingAttitudeControl::task_main()
 	vehicle_manual_poll();
 	vehicle_status_poll();
 
-	/* wakeup source(s) */
+	/* wakeup source */
-	struct pollfd fds[2];
+	px4_pollfd_struct_t fds[2];
 
 	/* Setup of loop */
 	fds[0].fd = _params_sub;
@@ -660,11 +660,10 @@ FixedwingAttitudeControl::task_main()
 	_task_running = true;
 
 	while (!_task_should_exit) {
-
 		static int loop_counter = 0;
 
 		/* wait for up to 500ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit, etc. */
 		if (pret == 0) {
@@ -691,7 +690,6 @@ FixedwingAttitudeControl::task_main()
 
 		/* only run controller if attitude changed */
 		if (fds[1].revents & POLLIN) {
-
 			static uint64_t last_run = 0;
 			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
 			last_run = hrt_absolute_time();
@@ -703,6 +701,7 @@ FixedwingAttitudeControl::task_main()
 			/* load local copies */
 			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
 
+
 			if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
 				/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
 				 *

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1719,7 +1719,7 @@ FixedwingPositionControl::task_main()
 	}
 
 	/* wakeup source(s) */
-	struct pollfd fds[2];
+	px4_pollfd_struct_t fds[2];
 
 	/* Setup of loop */
 	fds[0].fd = _params_sub;
@@ -1732,7 +1732,7 @@ FixedwingPositionControl::task_main()
 	while (!_task_should_exit) {
 
 		/* wait for up to 500ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit, etc. */
 		if (pret == 0) {

src/modules/mavlink/mavlink_main.cpp

@@ -94,6 +94,7 @@
 #endif
 static const int ERROR = -1;
 
+#define DEFAULT_REMOTE_PORT_UDP			14550 ///< GCS port per MAVLink spec
 #define DEFAULT_DEVICE_NAME			"/dev/ttyS1"
 #define MAX_DATA_RATE				10000000	///< max data rate in bytes/s
 #define MAIN_LOOP_DELAY 			10000	///< 100 Hz @ 1000 bytes/s data rate
@@ -153,7 +154,6 @@ Mavlink::Mavlink() :
 	_receive_thread {},
 	_verbose(false),
 	_forwarding_on(false),
-	_passing_on(false),
 	_ftp_on(false),
 #ifndef __PX4_POSIX
 	_uart_fd(-1),
@@ -885,6 +885,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 		ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
 	} else if (get_protocol() == TCP) {
 		// not implemented, but possible to do so
+		warnx("TCP transport pending implementation");
 	}
 #endif
 
@@ -974,11 +975,12 @@ Mavlink::init_udp()
 		return;
 	}
 
-	unsigned char inbuf[256];
+	// set default target address
-	socklen_t addrlen = sizeof(_src_addr);
+	memset((char *)&_src_addr, 0, sizeof(_src_addr));
+	_src_addr.sin_family = AF_INET;
+	inet_aton("127.0.0.1", &_src_addr.sin_addr);
+	_src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP);
 
-	// wait for client to connect to socket
-	recvfrom(_socket_fd,inbuf,sizeof(inbuf),0,(struct sockaddr *)&_src_addr,&addrlen);
 #endif
 }
 
@@ -1319,7 +1321,7 @@ Mavlink::message_buffer_mark_read(int n)
 void
 Mavlink::pass_message(const mavlink_message_t *msg)
 {
-	if (_passing_on) {
+	if (_forwarding_on) {
 		/* size is 8 bytes plus variable payload */
 		int size = MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len;
 		pthread_mutex_lock(&_message_buffer_mutex);
@@ -1494,10 +1496,6 @@ Mavlink::task_main(int argc, char *argv[])
 			_forwarding_on = true;
 			break;
 
-		case 'p':
-			_passing_on = true;
-			break;
-
 		case 'v':
 			_verbose = true;
 			break;
@@ -1543,11 +1541,6 @@ Mavlink::task_main(int argc, char *argv[])
 	/* flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
 
-	/* init socket if necessary */
-	if (get_protocol() == UDP) {
-		init_udp();
-	}
-
 #ifndef __PX4_POSIX
 	struct termios uart_config_original;
 
@@ -1568,7 +1561,7 @@ Mavlink::task_main(int argc, char *argv[])
 	mavlink_logbuffer_init(&_logbuffer, 5);
 
 	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
-	if (_passing_on || _ftp_on) {
+	if (_forwarding_on || _ftp_on) {
 		/* initialize message buffer if multiplexing is on or its needed for FTP.
 		 * make space for two messages plus off-by-one space as we use the empty element
 		 * marker ring buffer approach.
@@ -1724,6 +1717,11 @@ Mavlink::task_main(int argc, char *argv[])
 	/* now the instance is fully initialized and we can bump the instance count */
 	LL_APPEND(_mavlink_instances, this);
 
+	/* init socket if necessary */
+	if (get_protocol() == UDP) {
+		init_udp();
+	}
+
 	/* if the protocol is serial, we send the system version blindly */
 	if (get_protocol() == SERIAL) {
 		send_autopilot_capabilites();
@@ -1835,7 +1833,7 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 
 		/* pass messages from other UARTs or FTP worker */
-		if (_passing_on || _ftp_on) {
+		if (_forwarding_on || _ftp_on) {
 
 			bool is_part;
 			uint8_t *read_ptr;
@@ -1944,7 +1942,7 @@ Mavlink::task_main(int argc, char *argv[])
 	/* close mavlink logging device */
 	px4_close(_mavlink_fd);
 
-	if (_passing_on || _ftp_on) {
+	if (_forwarding_on || _ftp_on) {
 		message_buffer_destroy();
 		pthread_mutex_destroy(&_message_buffer_mutex);
 	}

src/modules/mavlink/mavlink_main.h

@@ -47,6 +47,7 @@
 #else
 #include <sys/socket.h>
 #include <netinet/in.h>
+#include <arpa/inet.h>
 #include <drivers/device/device.h>
 #endif
 #include <systemlib/param/param.h>
@@ -333,7 +334,9 @@ public:
 	unsigned short		get_network_port() { return _network_port; }
 
 	int 			get_socket_fd () { return _socket_fd; };
-
+#ifdef __PX4_POSIX
+	struct sockaddr_in * get_client_source_address() {return &_src_addr;};
+#endif
 	static bool		boot_complete() { return _boot_complete; }
 
 protected:
@@ -376,7 +379,6 @@ private:
 
 	bool			_verbose;
 	bool			_forwarding_on;
-	bool			_passing_on;
 	bool			_ftp_on;
 #ifndef __PX4_QURT
 	int			_uart_fd;

src/modules/mavlink/mavlink_messages.cpp

@@ -2057,7 +2057,14 @@ protected:
 			msg.y = manual.y * 1000;
 			msg.z = manual.z * 1000;
 			msg.r = manual.r * 1000;
+			unsigned shift = 2;
 			msg.buttons = 0;
+			msg.buttons |= (manual.mode_switch << (shift * 0));
+			msg.buttons |= (manual.return_switch << (shift * 1));
+			msg.buttons |= (manual.posctl_switch << (shift * 2));
+			msg.buttons |= (manual.loiter_switch << (shift * 3));
+			msg.buttons |= (manual.acro_switch << (shift * 4));
+			msg.buttons |= (manual.offboard_switch << (shift * 5));
 
 			_mavlink->send_message(MAVLINK_MSG_ID_MANUAL_CONTROL, &msg);
 		}

src/modules/mavlink/mavlink_mission.cpp

@@ -479,8 +479,6 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
 
 			} else {
 				if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); }
-
-				_mavlink->send_statustext_info("WPM: mission is empty");
 			}
 
 			send_mission_count(msg->sysid, msg->compid, _count);

src/modules/mavlink/mavlink_receiver.cpp

@@ -990,8 +990,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 switch_pos_t
 MavlinkReceiver::decode_switch_pos(uint16_t buttons, unsigned sw)
 {
-	// XXX non-standard 3 pos switch decoding
+	// This 2-bit method should be used in the future: (buttons >> (sw * 2)) & 3;
-	return (buttons >> (sw * 2)) & 3;
+	return (buttons & (1 << sw)) ? manual_control_setpoint_s::SWITCH_POS_ON : manual_control_setpoint_s::SWITCH_POS_OFF;
 }
 
 int
@@ -1101,26 +1101,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 		rc.values[0] = man.x / 2 + 1500;
 		/* pitch */
 		rc.values[1] = man.y / 2 + 1500;
-
+		/* yaw */
-		/*
+		rc.values[2] = man.r / 2 + 1500;
-		 * yaw needs special handling as some joysticks have a circular mechanical mask,
-		 * which makes the corner positions unreachable.
-		 * scale yaw up and clip it to overcome this.
-		 */
-		rc.values[2] = man.r / 1.1f + 1500;
-		if (rc.values[2] > 2000) {
-			rc.values[2] = 2000;
-		} else if (rc.values[2] < 1000) {
-			rc.values[2] = 1000;
-		}
-
 		/* throttle */
-		rc.values[3] = man.z / 0.9f + 1000;
+		rc.values[3] = man.z / 1 + 1000;
-		if (rc.values[3] > 2000) {
-			rc.values[3] = 2000;
-		} else if (rc.values[3] < 1000) {
-			rc.values[3] = 1000;
-		}
 
 		/* decode all switches which fit into the channel mask */
 		unsigned max_switch = (sizeof(man.buttons) * 8);
@@ -1762,10 +1746,17 @@ MavlinkReceiver::receive_thread(void *arg)
 #ifdef __PX4_POSIX
 	struct sockaddr_in srcaddr;
 	socklen_t addrlen = sizeof(srcaddr);
+
 	if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) {
+		// make sure mavlink app has booted before we start using the socket
+		while (!_mavlink->boot_complete()) {
+			usleep(100000);
+		}
+
 		fds[0].fd = _mavlink->get_socket_fd();
 		fds[0].events = POLLIN;
 	}
+
 #endif
 	ssize_t nread = 0;
 
@@ -1780,13 +1771,19 @@ MavlinkReceiver::receive_thread(void *arg)
 			}
 #ifdef __PX4_POSIX
 			if (_mavlink->get_protocol() == UDP) {
-
 				if (fds[0].revents & POLLIN) {
 					nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen);
 				}
 			} else {
 				// could be TCP or other protocol
 			}
+
+			struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address();
+			int localhost = (127 << 24) + 1;
+			if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) {
+				// if we were sending to localhost before but have a new host then accept him
+				memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr));
+			}
 #endif
 			/* if read failed, this loop won't execute */
 			for (ssize_t i = 0; i < nread; i++) {

src/modules/mc_att_control/mc_att_control_params.c

@@ -36,8 +36,8 @@
  * Parameters for multicopter attitude controller.
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
- * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Anton Babushkin <anton@px4.io>
  */
 
 #include <systemlib/param/param.h>
@@ -60,7 +60,7 @@ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.12f);
+PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.15f);
 
 /**
  * Roll rate I gain
@@ -111,7 +111,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.12f);
+PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.15f);
 
 /**
  * Pitch rate I gain

src/modules/sdlog2/sdlog2.c

@@ -1272,13 +1272,16 @@ int sdlog2_thread_main(int argc, char *argv[])
 	
 	subs.sat_info_sub = -1;
 
-	/* close non-needed fd's */
+#ifdef __PX4_NUTTX
+	/* close non-needed fd's. We cannot do this for posix since the file
+	   descriptors will also be closed for the parent process
+	*/
 
 	/* close stdin */
 	close(0);
 	/* close stdout */
 	close(1);
-
+#endif
 	/* initialize thread synchronization */
 	pthread_mutex_init(&logbuffer_mutex, NULL);
 	pthread_cond_init(&logbuffer_cond, NULL);

src/modules/simulator/simulator.cpp

@@ -86,6 +86,11 @@ bool Simulator::getGPSSample(uint8_t *buf, int len)
 	return _gps.copyData(buf, len);
 }
 
+bool Simulator::getAirspeedSample(uint8_t *buf, int len)
+{
+	return _airspeed.copyData(buf, len);
+}
+
 void Simulator::write_MPU_data(void *buf) {
 	_mpu.writeData(buf);
 }
@@ -106,6 +111,10 @@ void Simulator::write_gps_data(void *buf) {
 	_gps.writeData(buf);
 }
 
+void Simulator::write_airspeed_data(void *buf) {
+	_airspeed.writeData(buf);
+}
+
 int Simulator::start(int argc, char *argv[])
 {
 	int ret = 0;

src/modules/simulator/simulator.h

@@ -43,6 +43,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
 #include <drivers/drv_baro.h>
@@ -93,6 +94,13 @@ struct RawBaroData {
 };
 #pragma pack(pop)
 
+#pragma pack(push, 1)
+struct RawAirspeedData {
+	float temperature;
+	float diff_pressure;
+};
+#pragma pack(pop)
+
 #pragma pack(push, 1)
 struct RawGPSData {
  int32_t lat;
@@ -190,12 +198,14 @@ public:
 	bool getMPUReport(uint8_t *buf, int len);
 	bool getBaroSample(uint8_t *buf, int len);
 	bool getGPSSample(uint8_t *buf, int len);
+	bool getAirspeedSample(uint8_t *buf, int len);
 
 	void write_MPU_data(void *buf);
 	void write_accel_data(void *buf);
 	void write_mag_data(void *buf);
 	void write_baro_data(void *buf);
 	void write_gps_data(void *buf);
+	void write_airspeed_data(void *buf);
 
 	bool isInitialized() { return _initialized; }
 
@@ -206,6 +216,7 @@ private:
 	_baro(1),
 	_mag(1),
 	_gps(1),
+	_airspeed(1),
 	_accel_pub(nullptr),
 	_baro_pub(nullptr),
 	_gyro_pub(nullptr),
@@ -217,10 +228,12 @@ private:
 	_actuator_outputs_sub(-1),
 	_vehicle_attitude_sub(-1),
 	_manual_sub(-1),
+	_vehicle_status_sub(-1),
 	_rc_input{},
 	_actuators{},
 	_attitude{},
-	_manual{}
+	_manual{},
+	_vehicle_status{}
 #endif
 	{}
 	~Simulator() { _instance=NULL; }
@@ -235,6 +248,7 @@ private:
 	simulator::Report<simulator::RawBaroData>	_baro;
 	simulator::Report<simulator::RawMagData>	_mag;
 	simulator::Report<simulator::RawGPSData>	_gps;
+	simulator::Report<simulator::RawAirspeedData> _airspeed;
 
 	// uORB publisher handlers
 	orb_advert_t _accel_pub;
@@ -255,14 +269,16 @@ private:
 	int _actuator_outputs_sub;
 	int _vehicle_attitude_sub;
 	int _manual_sub;
+	int _vehicle_status_sub;
 
 	// uORB data containers
 	struct rc_input_values _rc_input;
 	struct actuator_outputs_s _actuators;
 	struct vehicle_attitude_s _attitude;
 	struct manual_control_setpoint_s _manual;
+	struct vehicle_status_s _vehicle_status;
 
-	void poll_actuators();
+	void poll_topics();
 	void handle_message(mavlink_message_t *msg, bool publish);
 	void send_controls();
 	void pollForMAVLinkMessages(bool publish);

src/modules/simulator/simulator_mavlink.cpp

@@ -64,33 +64,46 @@ static socklen_t _addrlen = sizeof(_srcaddr);
 using namespace simulator;
 
 void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
-	float out[8];
+	float out[8] = {};
 
 	const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
 
 	// for now we only support quadrotors
 	unsigned n = 4;
 
-	for (unsigned i = 0; i < 8; i++) {
+	if (_vehicle_status.is_rotary_wing) {
-		if (_actuators.output[i] > PWM_LOWEST_MIN / 2) {
+		for (unsigned i = 0; i < 8; i++) {
-			if (i < n) {
+			if (_actuators.output[i] > PWM_LOWEST_MIN / 2) {
-				// scale PWM out 900..2100 us to 0..1 for rotors */
+				if (i < n) {
-				out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+					// scale PWM out 900..2100 us to 0..1 for rotors */
+					out[i] = (_actuators.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+
+				} else {
+					// scale PWM out 900..2100 us to -1..1 for other channels */
+					out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+				}
 
 			} else {
-				// scale PWM out 900..2100 us to -1..1 for other channels */
+				// send 0 when disarmed and for disabled channels */
-				out[i] = (_actuators.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+				out[i] = 0.0f;
 			}
-
-		} else {
-			// send 0 when disarmed and for disabled channels */
-			out[i] = 0.0f;
 		}
+	} else {
+		// convert back to range [-1, 1]
+		for (unsigned i = 0; i < 8; i++) {
+			out[i] = (_actuators.output[i] - 1500) / 600.0f;
+		}
+	}
+
+	// if vehicle status has not yet been updated, set actuator commands to zero
+	// this is to prevent the simulation getting into a bad state
+	if (_vehicle_status.timestamp == 0) {
+		memset(out, 0, sizeof(out));
 	}
 
 	actuator_msg.time_usec = hrt_absolute_time();
 	actuator_msg.roll_ailerons = out[0];
-	actuator_msg.pitch_elevator = out[1];
+	actuator_msg.pitch_elevator = _vehicle_status.is_rotary_wing ? out[1] : -out[1];
 	actuator_msg.yaw_rudder = out[2];
 	actuator_msg.throttle = out[3];
 	actuator_msg.aux1 = out[4];
@@ -174,11 +187,17 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) {
 
 	RawBaroData baro;
 	// calculate air pressure from altitude (valid for low altitude)
-	baro.pressure = PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt;
+	baro.pressure = (PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt) / 100.0f; // convert from Pa to mbar
 	baro.altitude = imu->pressure_alt;
 	baro.temperature = imu->temperature;
 
 	write_baro_data((void *)&baro);
+
+	RawAirspeedData airspeed;
+	airspeed.temperature = imu->temperature;
+	airspeed.diff_pressure = imu->diff_pressure;
+
+	write_airspeed_data((void *)&airspeed);
 }
 
 void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {
@@ -270,14 +289,18 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8
 	}
 }
 
-void Simulator::poll_actuators() {
+void Simulator::poll_topics() {
 	// copy new actuator data if available
 	bool updated;
 	orb_check(_actuator_outputs_sub, &updated);
-	if(updated) {
+	if (updated) {
-	  //PX4_WARN("Received actuator_output0 orb_topic");
 		orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators);
 	}
+
+	orb_check(_vehicle_status_sub, &updated);
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+	}
 }
 
 void *Simulator::sending_trampoline(void *) {
@@ -310,7 +333,7 @@ void Simulator::send() {
 
 		if (fds[0].revents & POLLIN) {
 			// got new data to read, update all topics
-			poll_actuators();
+			poll_topics();
 			send_controls();
 		}
 	}
@@ -419,6 +442,7 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 
 	// subscribe to topics
 	_actuator_outputs_sub = orb_subscribe_multi(ORB_ID(actuator_outputs), 0);
+	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	// got data from simulator, now activate the sending thread
 	pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL);

src/modules/systemlib/perf_counter.c

@@ -396,13 +396,12 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 		float rms = sqrtf(pce->M2 / (pce->event_count-1));
-
 		dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n",
 			handle->name,
 			(unsigned long long)pce->event_count,
 			(unsigned long long)pce->event_overruns,
 			(unsigned long long)pce->time_total,
-			(unsigned long long)pce->time_total / pce->event_count,
+			pce->event_count == 0 ? 0 : (unsigned long long)pce->time_total / pce->event_count,
 			(unsigned long long)pce->time_least,
 			(unsigned long long)pce->time_most,
 			(double)(1e6f * rms));

src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp

@@ -35,14 +35,12 @@
  * @file ets_airspeed.cpp
  * @author Simon Wilks
  * @author Mark Charlebois (simulator)
- *
+ * @author Roman Bapst (simulator)
- * Driver for the Simulated Airspeed Sensor based on Eagle Tree Airspeed V3.
+ * Driver for the Simulated AirspeedSim Sensor based on Eagle Tree AirspeedSim V3.
  */
 
 #include <px4_config.h>
 
-#include <drivers/device/i2c.h>
-
 #include <sys/types.h>
 #include <stdint.h>
 #include <stdlib.h>
@@ -56,8 +54,6 @@
 #include <math.h>
 #include <unistd.h>
 
-#include <arch/board/board.h>
-
 #include <systemlib/airspeed.h>
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
@@ -71,12 +67,15 @@
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/subsystem_info.h>
 
-#include <drivers/airspeed/airspeed.h>
+#include <simulator/simulator.h>
 
-Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) :
+#include "airspeedsim.h"
-	I2C("Airspeed", path, bus, address),
+
+AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char* path) :
+	VDev("AIRSPEEDSIM", path),
 	_reports(nullptr),
 	_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
+	_retries(0),
 	_max_differential_pressure_pa(0),
 	_sensor_ok(false),
 	_last_published_sensor_ok(true), /* initialize differently to force publication */
@@ -97,7 +96,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha
 	memset(&_work, 0, sizeof(_work));
 }
 
-Airspeed::~Airspeed()
+AirspeedSim::~AirspeedSim()
 {
 	/* make sure we are truly inactive */
 	stop();
@@ -116,18 +115,21 @@ Airspeed::~Airspeed()
 }
 
 int
-Airspeed::init()
+AirspeedSim::init()
 {
 	int ret = ERROR;
 
-	/* do I2C init (and probe) first */
+	/* init base class */
-	if (I2C::init() != OK)
+	if (VDev::init() != OK) {
+		DEVICE_DEBUG("VDev init failed");
 		goto out;
+	}
 
 	/* allocate basic report buffers */
 	_reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s));
-	if (_reports == nullptr)
+	if (_reports == nullptr) {
 		goto out;
+	}
 
 	/* register alternate interfaces if we have to */
 	_class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH);
@@ -154,7 +156,7 @@ out:
 }
 
 int
-Airspeed::probe()
+AirspeedSim::probe()
 {
 	/* on initial power up the device may need more than one retry
 	   for detection. Once it is running the number of retries can
@@ -169,7 +171,7 @@ Airspeed::probe()
 }
 
 int
-Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
+AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
 
@@ -272,12 +274,13 @@ Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
 
 	default:
 		/* give it to the superclass */
-		return I2C::ioctl(filp, cmd, arg);
+		//return I2C::ioctl(filp, cmd, arg);	XXX SIM should be super class
+		return 0;
 	}
 }
 
 ssize_t
-Airspeed::read(device::file_t *filp, char *buffer, size_t buflen)
+AirspeedSim::read(device::file_t *filp, char *buffer, size_t buflen)
 {
 	unsigned count = buflen / sizeof(differential_pressure_s);
 	differential_pressure_s *abuf = reinterpret_cast<differential_pressure_s *>(buffer);
@@ -336,24 +339,24 @@ Airspeed::read(device::file_t *filp, char *buffer, size_t buflen)
 }
 
 void
-Airspeed::start()
+AirspeedSim::start()
 {
 	/* reset the report ring and state machine */
 	_collect_phase = false;
 	_reports->flush();
 
 	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1);
+	work_queue(HPWORK, &_work, (worker_t)&AirspeedSim::cycle_trampoline, this, 1);
 }
 
 void
-Airspeed::stop()
+AirspeedSim::stop()
 {
 	work_cancel(HPWORK, &_work);
 }
 
 void
-Airspeed::update_status()
+AirspeedSim::update_status()
 {
 	if (_sensor_ok != _last_published_sensor_ok) {
 		/* notify about state change */
@@ -375,9 +378,9 @@ Airspeed::update_status()
 }
 
 void
-Airspeed::cycle_trampoline(void *arg)
+AirspeedSim::cycle_trampoline(void *arg)
 {
-	Airspeed *dev = (Airspeed *)arg;
+	AirspeedSim *dev = (AirspeedSim *)arg;
 
 	dev->cycle();
 	// XXX we do not know if this is
@@ -387,7 +390,7 @@ Airspeed::cycle_trampoline(void *arg)
 }
 
 void
-Airspeed::print_info()
+AirspeedSim::print_info()
 {
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_comms_errors);
@@ -397,8 +400,26 @@ Airspeed::print_info()
 }
 
 void
-Airspeed::new_report(const differential_pressure_s &report)
+AirspeedSim::new_report(const differential_pressure_s &report)
 {
 	if (!_reports->force(&report))
 		perf_count(_buffer_overflows);
 }
+
+int
+AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) {
+	if (recv_len > 0) {
+		// this is equivalent to the collect phase
+		Simulator *sim = Simulator::getInstance();
+		if (sim == NULL) {
+			PX4_ERR("Error BARO_SIM::transfer no simulator");
+			return -ENODEV;
+		}
+		PX4_DEBUG("BARO_SIM::transfer getting sample");
+		sim->getAirspeedSample(recv, recv_len);
+	} else {
+		// we don't need measure phase
+	}
+
+	return 0;
+}

src/platforms/posix/drivers/airspeedsim/airspeedsim.h

@@ -40,7 +40,7 @@
 
 #include <px4_config.h>
 
-#include <drivers/device/i2c.h>
+//#include <drivers/device/i2c.h>
 
 #include <sys/types.h>
 #include <stdint.h>
@@ -55,10 +55,11 @@
 #include <math.h>
 #include <unistd.h>
 
-#include <nuttx/arch.h>
+//#include <nuttx/arch.h>
-#include <nuttx/wqueue.h>
+//#include <nuttx/wqueue.h>
-#include <nuttx/clock.h>
+//#include <nuttx/clock.h>
 
+#include <px4_workqueue.h>
 #include <arch/board/board.h>
 
 #include <systemlib/airspeed.h>
@@ -69,6 +70,7 @@
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/device/ringbuffer.h>
+#include <drivers/device/device.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
@@ -87,7 +89,7 @@ static const int ERROR = -1;
 # error This requires CONFIG_SCHED_WORKQUEUE.
 #endif
 
-class __EXPORT AirspeedSim : public device::I2C
+class __EXPORT AirspeedSim : public device::VDev
 {
 public:
 	AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path);
@@ -95,8 +97,8 @@ public:
 
 	virtual int	init();
 
-	virtual ssize_t	read(px4_dev_handle_t *handlep, char *buffer, size_t buflen);
+	virtual ssize_t	read(device::file_t *filp, char *buffer, size_t buflen);
-	virtual int	ioctl(px4_dev_handle_t *handlep, int cmd, unsigned long arg);
+	virtual int	ioctl(device::file_t *filp, int cmd, unsigned long arg);
 
 	/**
 	 * Diagnostics - print some basic information about the driver.
@@ -104,9 +106,11 @@ public:
 	virtual void	print_info();
 
 private:
-	RingBuffer		*_reports;
+	ringbuffer::RingBuffer		*_reports;
 	perf_counter_t		_buffer_overflows;
 
+	unsigned _retries;	// XXX this should come from the SIM class
+
 	/* this class has pointer data members and should not be copied */
 	AirspeedSim(const AirspeedSim &);
 	AirspeedSim &operator=(const AirspeedSim &);
@@ -122,16 +126,19 @@ protected:
 	virtual int	measure() = 0;
 	virtual int	collect() = 0;
 
+	virtual int	transfer(const uint8_t *send, unsigned send_len,
+					 uint8_t *recv, unsigned recv_len);
+
 	/**
 	 * Update the subsystem status
 	 */
 	void update_status();
 
-	work_s			_work;
+	struct work_s			_work;
 	float			_max_differential_pressure_pa;
 	bool			_sensor_ok;
 	bool			_last_published_sensor_ok;
-	int			_measure_ticks;
+	unsigned			_measure_ticks;
 	bool			_collect_phase;
 	float			_diff_pres_offset;
 

src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp

@@ -0,0 +1,631 @@
+/****************************************************************************
+ *
+ *   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 meas_airspeed_sim.cpp
+ * @author Lorenz Meier
+ * @author Sarthak Kaingade
+ * @author Simon Wilks
+ * @author Thomas Gubler
+ * @author Roman Bapst
+ *
+ * Driver for a simulated airspeed sensor.
+ *
+ */
+
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <systemlib/airspeed.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+
+#include <drivers/drv_airspeed.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/system_power.h>
+
+#include "airspeedsim.h"
+
+/* I2C bus address is 1010001x */
+#define I2C_ADDRESS_MS4525DO	0x28	/**< 7-bit address. Depends on the order code (this is for code "I") */
+#define PATH_MS4525		"/dev/ms4525"
+/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
+#define I2C_ADDRESS_MS5525DSO	0x77	//0x77/* 7-bit address, addr. pin pulled low */
+#define PATH_MS5525		"/dev/ms5525"
+
+/* Register address */
+#define ADDR_READ_MR			0x00	/* write to this address to start conversion */
+
+/* Measurement rate is 100Hz */
+#define MEAS_RATE 100
+#define MEAS_DRIVER_FILTER_FREQ 1.2f
+#define CONVERSION_INTERVAL	(1000000 / MEAS_RATE)	/* microseconds */
+
+class MEASAirspeedSim : public AirspeedSim
+{
+public:
+	MEASAirspeedSim(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525);
+
+protected:
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	virtual void	cycle();
+	virtual int	measure();
+	virtual int	collect();
+
+	math::LowPassFilter2p	_filter;
+
+	/**
+	 * Correct for 5V rail voltage variations
+	 */
+	void voltage_correction(float &diff_pres_pa, float &temperature);
+
+	int _t_system_power;
+	struct system_power_s system_power;
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int measairspeedsim_main(int argc, char *argv[]);
+
+MEASAirspeedSim::MEASAirspeedSim(int bus, int address, const char *path) : AirspeedSim(bus, address,
+			CONVERSION_INTERVAL, path),
+	_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
+	_t_system_power(-1),
+	system_power{}
+{}
+
+int
+MEASAirspeedSim::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	uint8_t cmd = 0;
+	ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+	}
+
+	return ret;
+}
+
+int
+MEASAirspeedSim::collect()
+{
+	int	ret = -EIO;
+
+	/* read from the sensor */
+	#pragma pack(push, 1)
+	struct {
+		float		temperature;
+		float		diff_pressure;
+	} airspeed_report;
+	#pragma pack(pop)
+
+
+	perf_begin(_sample_perf);
+
+	ret = transfer(nullptr, 0, (uint8_t *)&airspeed_report, sizeof(airspeed_report));
+
+	if (ret < 0) {
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+
+	uint8_t status = 0;
+
+	switch (status) {
+	case 0:
+		break;
+
+	case 1:
+
+	/* fallthrough */
+	case 2:
+
+	/* fallthrough */
+	case 3:
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return -EAGAIN;
+	}
+
+	float temperature = airspeed_report.temperature;
+	float diff_press_pa_raw = airspeed_report.diff_pressure * 100.0f; // convert from millibar to bar
+
+	struct differential_pressure_s report;
+
+	/* track maximum differential pressure measured (so we can work out top speed). */
+	if (diff_press_pa_raw > _max_differential_pressure_pa) {
+		_max_differential_pressure_pa = diff_press_pa_raw;
+	}
+
+	report.timestamp = hrt_absolute_time();
+	report.error_count = perf_event_count(_comms_errors);
+	report.temperature = temperature;
+	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa_raw);
+
+	report.differential_pressure_raw_pa = diff_press_pa_raw;
+	report.max_differential_pressure_pa = _max_differential_pressure_pa;
+
+	if (_airspeed_pub != nullptr && !(_pub_blocked)) {
+		/* publish it */
+		orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
+	}
+
+	new_report(report);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+
+	return ret;
+}
+
+void
+MEASAirspeedSim::cycle()
+{
+	int ret;
+
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		ret = collect();
+
+		if (OK != ret) {
+			/* restart the measurement state machine */
+			start();
+			_sensor_ok = false;
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&AirspeedSim::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	ret = measure();
+
+	if (OK != ret) {
+		DEVICE_DEBUG("measure error");
+	}
+
+	_sensor_ok = (ret == OK);
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&AirspeedSim::cycle_trampoline,
+		   this,
+		   USEC2TICK(CONVERSION_INTERVAL));
+}
+
+/**
+   correct for 5V rail voltage if the system_power ORB topic is
+   available
+
+   See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
+   offset versus voltage for 3 sensors
+ */
+void
+MEASAirspeedSim::voltage_correction(float &diff_press_pa, float &temperature)
+{
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+
+	if (_t_system_power == -1) {
+		_t_system_power = orb_subscribe(ORB_ID(system_power));
+	}
+
+	if (_t_system_power == -1) {
+		// not available
+		return;
+	}
+
+	bool updated = false;
+	orb_check(_t_system_power, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(system_power), _t_system_power, &system_power);
+	}
+
+	if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) {
+		// not valid, skip correction
+		return;
+	}
+
+	const float slope = 65.0f;
+	/*
+	  apply a piecewise linear correction, flattening at 0.5V from 5V
+	 */
+	float voltage_diff = system_power.voltage5V_v - 5.0f;
+
+	if (voltage_diff > 0.5f) {
+		voltage_diff = 0.5f;
+	}
+
+	if (voltage_diff < -0.5f) {
+		voltage_diff = -0.5f;
+	}
+
+	diff_press_pa -= voltage_diff * slope;
+
+	/*
+	  the temperature masurement varies as well
+	 */
+	const float temp_slope = 0.887f;
+	voltage_diff = system_power.voltage5V_v - 5.0f;
+
+	if (voltage_diff > 0.5f) {
+		voltage_diff = 0.5f;
+	}
+
+	if (voltage_diff < -1.0f) {
+		voltage_diff = -1.0f;
+	}
+
+	temperature -= voltage_diff * temp_slope;
+#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace meas_airspeed_sim
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+MEASAirspeedSim	*g_dev = nullptr;
+
+int	start(int i2c_bus);
+int	stop();
+int test();
+int	reset();
+int	info();
+
+/**
+ * Start the driver.
+ *
+ * This function call only returns once the driver is up and running
+ * or failed to detect the sensor.
+ */
+int
+start(int i2c_bus)
+{
+	int fd;
+
+	if (g_dev != nullptr) {
+		PX4_WARN("already started");
+	}
+
+	/* create the driver, try the MS4525DO first */
+	g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525);
+
+	/* check if the MS4525DO was instantiated */
+	if (g_dev == nullptr) {
+		goto fail;
+	}
+
+	/* try the MS5525DSO next if init fails */
+	if (OK != g_dev->AirspeedSim::init()) {
+		delete g_dev;
+		g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525);
+
+		/* check if the MS5525DSO was instantiated */
+		if (g_dev == nullptr) {
+			goto fail;
+		}
+
+		/* both versions failed if the init for the MS5525DSO fails, give up */
+		if (OK != g_dev->AirspeedSim::init()) {
+			goto fail;
+		}
+	}
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = px4_open(PATH_MS4525, O_RDONLY);
+
+	if (fd < 0) {
+		goto fail;
+	}
+
+	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		goto fail;
+	}
+
+	return 0;
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	PX4_ERR("no MS4525 airspeedSim sensor connected");
+	return 1;
+}
+
+/**
+ * Stop the driver
+ */
+int
+stop()
+{
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+
+	} else {
+		PX4_ERR("driver not running");
+	}
+
+	return 0;
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+int
+test()
+{
+	struct differential_pressure_s report;
+	ssize_t sz;
+	int ret;
+
+	int fd = px4_open(PATH_MS4525, O_RDONLY);
+
+	if (fd < 0) {
+		PX4_ERR("%s open failed (try 'meas_airspeed_sim start' if the driver is not running", PATH_MS4525);
+			return 1;
+	}
+
+	/* do a simple demand read */
+	sz = px4_read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report)) {
+		PX4_ERR("immediate read failed");
+		return 1;
+	}
+
+	PX4_WARN("single read");
+	PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
+
+	/* start the sensor polling at 2Hz */
+	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
+		PX4_WARN("failed to set 2Hz poll rate");
+	}
+
+	/* read the sensor 5x and report each value */
+	for (unsigned i = 0; i < 5; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		if (ret != 1) {
+			warnx("timed out");
+		}
+
+		/* now go get it */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			PX4_WARN("periodic read failed");
+		}
+
+		PX4_WARN("periodic read %u", i);
+		PX4_WARN("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
+		PX4_WARN("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
+	}
+
+	/* reset the sensor polling to its default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
+		PX4_ERR("failed to set default rate");
+		return 1;
+	}
+
+	PX4_WARN("PASS");
+
+	return 0;
+}
+
+/**
+ * Reset the driver.
+ */
+int
+reset()
+{
+	int fd = open(PATH_MS4525, O_RDONLY);
+
+	if (fd < 0) {
+		PX4_ERR("failed ");
+		return 1;
+	}
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
+		PX4_ERR("driver reset failed");
+		return 1;
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		PX4_ERR("driver poll restart failed");
+		return 1;
+	}
+
+	return 0;
+}
+
+/**
+ * Print a little info about the driver.
+ */
+int
+info()
+{
+	if (g_dev == nullptr) {
+		PX4_WARN("driver not running");
+	}
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	return 0;
+}
+
+} // namespace
+
+
+static void
+meas_airspeed_usage()
+{
+	PX4_WARN("usage: meas_airspeed_sim command [options]");
+	PX4_WARN("options:");
+	PX4_WARN("\t-b --bus i2cbus (%d)", 1);
+	PX4_WARN("command:");
+	PX4_WARN("\tstart|stop|reset|test|info");
+}
+
+int
+measairspeedsim_main(int argc, char *argv[])
+{
+	int i2c_bus = 1;//PX4_I2C_BUS_DEFAULT;
+
+	int i;
+
+	for (i = 1; i < argc; i++) {
+		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
+			if (argc > i + 1) {
+				i2c_bus = atoi(argv[i + 1]);
+			}
+		}
+	}
+
+	int ret;
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start")) {
+		ret = meas_airspeed_sim::start(i2c_bus);
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop")) {
+		ret = meas_airspeed_sim::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test")) {
+		ret = meas_airspeed_sim::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset")) {
+		ret = meas_airspeed_sim::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
+		ret = meas_airspeed_sim::info();
+	}
+
+	meas_airspeed_usage();
+	return ret;
+}

src/platforms/posix/drivers/airspeedsim/module.mk

@@ -34,7 +34,8 @@
 #
 # Makefile to build the generic airspeed driver.
 #
+MODULE_COMMAND	= measairspeedsim
 
-SRCS			= airspeedsim.cpp
+SRCS			= airspeedsim.cpp meas_airspeed_sim.cpp
 
 MAXOPTIMIZATION	 = -Os

src/platforms/posix/main.cpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 /**
  * @file main.cpp
- * Basic shell to execute builtin "apps" 
+ * Basic shell to execute builtin "apps"
  *
  * @author Mark Charlebois <charlebm@gmail.com>
  */
@@ -42,8 +42,10 @@
 #include <string>
 #include <sstream>
 #include <vector>
+#include <signal.h>
 
-namespace px4 {
+namespace px4
+{
 void init_once(void);
 }
 
@@ -54,21 +56,34 @@ typedef int (*px4_main_t)(int argc, char *argv[]);
 #include "apps.h"
 #include "px4_middleware.h"
 
-static void run_cmd(const vector<string> &appargs) {
+static bool _ExitFlag = false;
+extern "C" {
+	void _SigIntHandler(int sig_num);
+	void _SigIntHandler(int sig_num)
+	{
+		_ExitFlag = true;
+	}
+}
+
+static void run_cmd(const vector<string> &appargs)
+{
 	// command is appargs[0]
 	string command = appargs[0];
 	cout << "----------------------------------\n";
+
 	if (apps.find(command) != apps.end()) {
-		const char *arg[appargs.size()+2];
+		const char *arg[appargs.size() + 2];
 
 		unsigned int i = 0;
+
 		while (i < appargs.size() && appargs[i] != "") {
 			arg[i] = (char *)appargs[i].c_str();
 			++i;
 		}
+
 		arg[i] = (char *)0;
 		cout << "Running: " << command << "\n";
-		apps[command](i,(char **)arg);
+		apps[command](i, (char **)arg);
 		usleep(40000);
 		cout << "Returning: " << command << "\n";
 
@@ -78,40 +93,110 @@ static void run_cmd(const vector<string> &appargs) {
 	}
 }
 
+static void usage()
+{
+
+	cout << "./mainapp [-d] [startup_config] -h" << std::endl;
+	cout << "   -d            - Optional flag to run the app in daemon mode and does not take listen for user input." <<
+	     std::endl;
+	cout << "                   This is needed if mainapp is intended to be run as a upstart job on linux" << std::endl;
+	cout << "<startup_config> - config file for starting/stopping px4 modules" << std::endl;
+	cout << "   -h            - help/usage information" << std::endl;
+}
+
 static void process_line(string &line)
 {
 	vector<string> appargs(8);
 
-	stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >> appargs[7];
+	stringstream(line) >> appargs[0] >> appargs[1] >> appargs[2] >> appargs[3] >> appargs[4] >> appargs[5] >> appargs[6] >>
+			   appargs[7];
 	run_cmd(appargs);
 }
 
 int main(int argc, char **argv)
 {
-	px4::init_once();
+	bool daemon_mode = false;
+	signal(SIGINT, _SigIntHandler);
 
-	px4::init(argc, argv, "mainapp");
+	int index = 1;
+	bool error_detected = false;
+	char *commands_file = nullptr;
 
-	// Execute a command list of provided
+	while (index < argc) {
-	if (argc == 2) {
+		if (argv[index][0] == '-') {
-		ifstream infile(argv[1]);
+			// the arg starts with -
+			if (strcmp(argv[index], "-d") == 0) {
+				daemon_mode = true;
 
-		if (!infile) {
+			} else if (strcmp(argv[index], "-h") == 0) {
-			cout << "failed opening script" << argv[1] << std::endl;
+				usage();
-			return 1;
+				return 0;
+
+			} else {
+				PX4_WARN("Unknown/unhandled parameter: %s", argv[index]);
+				return 1;
+			}
+
+		} else {
+			// this is an argument that does not have '-' prefix; treat it like a file name
+			ifstream infile(argv[index]);
+
+			if (infile.good()) {
+				infile.close();
+				commands_file = argv[index];
+
+			} else {
+				PX4_WARN("Error opening file: %s", argv[index]);
+				error_detected = true;
+				break;
+			}
 		}
 
-		for (string line; getline(infile, line, '\n'); ) {
+		++index;
-			process_line(line);
-		}
 	}
 
-	string mystr;
+	if (!error_detected) {
-	
+		px4::init_once();
-	while(1) {
+
-		cout << "Enter a command and its args:" << endl;
+		px4::init(argc, argv, "mainapp");
-		getline (cin,mystr);
+
-		process_line(mystr);
+		//if commandfile is present, process the commands from the file
-		mystr = "";
+		if (commands_file != nullptr) {
+			ifstream infile(commands_file);
+
+			if (infile.is_open()) {
+				for (string line; getline(infile, line, '\n');) {
+					process_line(line);
+				}
+
+			} else {
+				PX4_WARN("Error opening file: %s", commands_file);
+			}
+		}
+
+		if (!daemon_mode) {
+			string mystr;
+
+			while (!_ExitFlag) {
+				cout << "Enter a command and its args:" << endl;
+				getline(cin, mystr);
+				process_line(mystr);
+				mystr = "";
+			}
+
+		} else {
+			while (!_ExitFlag) {
+				sleep(1000000);
+			}
+		}
+
+		if (px4_task_is_running("muorb")) {
+			// sending muorb stop is needed if it is running to exit cleanly
+			vector<string> muorb_stop_cmd = { "muorb", "stop" };
+			run_cmd(muorb_stop_cmd);
+		}
+
+		vector<string> shutdown_cmd = { "shutdown" };
+		run_cmd(shutdown_cmd);
 	}
 }

src/platforms/posix/px4_layer/px4_posix_tasks.cpp

@@ -280,6 +280,17 @@ void px4_show_tasks()
 
 }
 
+bool px4_task_is_running(const char *taskname)
+{
+	int idx;
+	for (idx=0; idx < PX4_MAX_TASKS; idx++)
+	{
+		if (taskmap[idx].isused && (strcmp(taskmap[idx].name.c_str(), taskname) == 0)) {
+			return true;
+		}
+	}
+	return false;
+}
 __BEGIN_DECLS
 
 unsigned long px4_getpid()

src/platforms/px4_tasks.h

@@ -117,5 +117,8 @@ __EXPORT void px4_task_exit(int ret);
 /** Show a list of running tasks **/
 __EXPORT void px4_show_tasks(void);
 
+/** See if a task is running **/
+__EXPORT bool px4_task_is_running(const char *taskname);
+
 __END_DECLS
 

unittests/CMakeLists.txt

@@ -11,6 +11,13 @@ CMAKE_FORCE_CXX_COMPILER( clang++ Clang )
 #set( CMAKE_FIND_ROOT_PATH_MODE_LIBARARY_ONLY )
 #set( CMAKE_FIND_ROOT_PATH_MODE_INCLUDE_ONLY )
 
+if("${CMAKE_C_COMPILER_ID}" STREQUAL "Clang")
+  add_compile_options(-Qunused-arguments)
+endif()
+if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+  add_compile_options(-Qunused-arguments)
+endif()
+
 project(unittests)
 enable_testing()
 
@@ -123,8 +130,7 @@ add_executable(mixer_test mixer_test.cpp hrt.cpp
                           ${PX_SRC}/modules/systemlib/mixer/mixer_simple.cpp
                           ${PX_SRC}/modules/systemlib/pwm_limit/pwm_limit.c
                           ${PX_SRC}/systemcmds/tests/test_mixer.cpp)
-
+target_link_libraries( mixer_test px4_platform )
-target_link_libraries( mixer_test LINK_PUBLIC px4_platform )
 
                           
 add_gtest(mixer_test)

unittests/sbus2_test.cpp

@@ -33,7 +33,7 @@ TEST(SBUS2Test, SBUS2)
 	// Init the parser
 	uint8_t frame[30];
 	unsigned partial_frame_count = 0;
-	uint16_t rc_values[18];
+	//uint16_t rc_values[18];
 	//uint16_t num_values;
 	//bool sbus_failsafe;
 	//bool sbus_frame_drop;

unittests/st24_test.cpp

@@ -41,7 +41,7 @@ TEST(ST24Test, ST24)
 		last_time = f;
 
 		// Pipe the data into the parser
-		hrt_abstime now = hrt_absolute_time();
+		//hrt_abstime now = hrt_absolute_time();
 
 		uint8_t rssi;
 		uint8_t rx_count;
@@ -53,8 +53,7 @@ TEST(ST24Test, ST24)
 			//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
 
 			for (unsigned i = 0; i < channel_count; i++) {
-
+				//int16_t val = channels[i];
-				int16_t val = channels[i];
 				//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
 			}
 		}

unittests/sumd_test.cpp

@@ -41,7 +41,7 @@ TEST(SUMDTest, SUMD)
 		last_time = f;
 
 		// Pipe the data into the parser
-		hrt_abstime now = hrt_absolute_time();
+		//hrt_abstime now = hrt_absolute_time();
 
 		uint8_t rssi;
 		uint8_t rx_count;
@@ -53,8 +53,7 @@ TEST(SUMDTest, SUMD)
 			//warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
 
 			for (unsigned i = 0; i < channel_count; i++) {
-
+				//int16_t val = channels[i];
-				int16_t val = channels[i];
 				//warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
 			}
 		}

unittests/uorb_unittests/uORBCommunicatorMock.hpp

@@ -126,8 +126,8 @@ class uORB_test::uORBCommunicatorMock : public uORBCommunicator::IChannel
 
  private:
   uORBCommunicator::IChannelRxHandler* _rx_handler;
-  int _sub_topicA_copy_fd;
+  //int _sub_topicA_copy_fd;
-  int _sub_topicB_copy_fd;
+  //int _sub_topicB_copy_fd;
 
   std::map<std::string, std::string> _topic_translation_map;