Merge branch 'master' into beta

Makefile

@@ -154,7 +154,8 @@ posix_sitl_ekf2:
 	$(call cmake-build,$@)
 
 ros_sitl_default:
-	$(call cmake-build,$@)
+	@echo "This target is deprecated. Use make 'posix_sitl_default gazebo' instead."
+#	$(call cmake-build,$@)
 
 qurt_eagle_travis:
 	$(call cmake-build,$@)
@@ -171,6 +172,12 @@ qurt_eagle_default:
 posix_eagle_default:
 	$(call cmake-build,$@)
 	
+posix_rpi2_default:
+	$(call cmake-build,$@)
+
+posix_rpi2_release:
+	$(call cmake-build,$@)
+
 posix: posix_sitl_default
 
 sitl_deprecation:

ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol

@@ -2,7 +2,7 @@
 #
 # @name Duorotor Tailsitter
 #
-# @type VTOL Tailsitter
+# @type VTOL Duo Tailsitter
 #
 # @maintainer Roman Bapst <roman@px4.io>
 #

ROMFS/px4fmu_common/init.d/13008_QuadRanger

@@ -18,13 +18,13 @@ then
 	param set PWM_AUX_REV1 1
 	param set PWM_AUX_REV2 1
 
-	param set MC_ROLL_P 6.5
+	param set MC_ROLL_P 7.0
-	param set MC_ROLLRATE_P 0.1
+	param set MC_ROLLRATE_P 0.25
 	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 6.5
+	param set MC_PITCH_P 7.0
-	param set MC_PITCHRATE_P 0.1
+	param set MC_PITCHRATE_P 0.23
 	param set MC_PITCHRATE_I 0.002
 	param set MC_PITCHRATE_D 0.003
 	param set MC_PITCHRATE_FF 0.0

ROMFS/px4fmu_common/init.d/rcS

@@ -550,7 +550,7 @@ then
 			# Avoid using ttyS1 for MAVLink on FMUv4
 			if ver hwcmp PX4FMU_V4
 			then
-				set MAVLINK_F "-r 1200 -d /dev/ttyS2"
+				set MAVLINK_F "-r 1200 -d /dev/ttyS1"
 				# Start MAVLink on Wifi (ESP8266 port)
 				mavlink start -r 800000 -b 921600 -d /dev/ttyS0
 			fi
@@ -560,6 +560,12 @@ then
 	mavlink start $MAVLINK_F
 	unset MAVLINK_F
 
+	if ver hwcmp PX4FMU_V4
+	then
+		# Start MAVLink on OSD (TELEM2 port)
+		mavlink start -r 1200 -d /dev/ttyS2 -b 57600 -m osd
+	fi
+
 	#
 	# MAVLink onboard / TELEM2
 	#
@@ -835,6 +841,11 @@ then
 	fi
 fi
 
+if ver hwcmp PX4FMU_V4
+then
+	frsky_telemetry start -d /dev/ttyS6
+fi
+
 if ver hwcmp PX4FMU_V2
 then
 	# Check for flow sensor - as it is a background task, launch it last

Tools/check_code_style.sh

@@ -25,7 +25,6 @@ for fn in $(find src/examples \
                  src/modules/uORB \
                  src/modules/bottle_drop \
                  src/modules/dataman \
-                 src/modules/fixedwing_backside \
                  src/modules/segway \
                  src/modules/local_position_estimator \
                  src/modules/unit_test \

Tools/generate_listener.py

@@ -136,12 +136,12 @@ extern "C" __EXPORT int listener_main(int argc, char *argv[]);
 int listener_main(int argc, char *argv[]) {
 	int sub = -1;
 	orb_id_t ID;
-	if(argc < 3) {
+	if(argc < 2) {
-		printf("need at least two arguments: topic name, number of messages to print\\n");
+		printf("need at least two arguments: topic name. [optional number of messages to print]\\n");
 		return 1;
 	}
 """)
-print("\tunsigned num_msgs = atoi(argv[2]);")
+print("\tunsigned num_msgs = (argc > 2) ? atoi(argv[2]) : 1;")
 print("\tif (strncmp(argv[1],\"%s\",50) == 0) {" % messages[0])
 print("\t\tsub = orb_subscribe(ORB_ID(%s));" % messages[0])
 print("\t\tID = ORB_ID(%s);" % messages[0])
@@ -154,9 +154,11 @@ for index,m in enumerate(messages[1:]):
 	print("\t\tstruct %s_s container;" % m)
 	print("\t\tmemset(&container, 0, sizeof(container));")
 	print("\t\tbool updated;")
-	print("\t\tfor (unsigned i = 0; i < num_msgs; i++) {")
+	print("\t\tunsigned i = 0;")
+	print("\t\twhile(i < num_msgs) {")
 	print("\t\t\torb_check(sub,&updated);")
-	print("\t\t\tupdated = true;")
+	print("\t\t\tif (i == 0) { updated = true; } else { usleep(500); }")
+	print("\t\t\ti++;")
 	print("\t\t\tif (updated) {")
 	print("\t\tprintf(\"\\nTOPIC: %s #%%d\\n\", i);" % m)
 	print("\t\t\torb_copy(ID,sub,&container);")
@@ -190,9 +192,15 @@ for index,m in enumerate(messages[1:]):
 		elif item[0] == "int32":
 			print("\t\t\tprintf(\"%s: %%d\\n\",container.%s);" % (item[1], item[1]))
 		elif item[0] == "uint32":
-			print("\t\t\tprintf(\"%s: %%d\\n\",container.%s);" % (item[1], item[1]))
-		elif item[0] == "uint8":
 			print("\t\t\tprintf(\"%s: %%u\\n\",container.%s);" % (item[1], item[1]))
+		elif item[0] == "int16":
+			print("\t\t\tprintf(\"%s: %%d\\n\",(int)container.%s);" % (item[1], item[1]))
+		elif item[0] == "uint16":
+			print("\t\t\tprintf(\"%s: %%u\\n\",(unsigned)container.%s);" % (item[1], item[1]))
+		elif item[0] == "int8":
+			print("\t\t\tprintf(\"%s: %%d\\n\",(int)container.%s);" % (item[1], item[1]))
+		elif item[0] == "uint8":
+			print("\t\t\tprintf(\"%s: %%u\\n\",(unsigned)container.%s);" % (item[1], item[1]))
 		elif item[0] == "bool":
 			print("\t\t\tprintf(\"%s: %%s\\n\",container.%s ? \"True\" : \"False\");" % (item[1], item[1]))
 	print("\t\t\t}")

Tools/px4airframes/xmlout.py

@@ -59,7 +59,7 @@ class XMLOutput():
                 xml_group.attrib["image"] = "VTOLPlane"
             elif (group.GetName() == "VTOL Quad Tailsitter"):
                 xml_group.attrib["image"] = "VTOLQuadRotorTailSitter"
-            elif (group.GetName() == "VTOLTiltRotor"):
+            elif (group.GetName() == "VTOL Tiltrotor"):
                 xml_group.attrib["image"] = "VTOLTiltRotor"
             elif (group.GetName() == "Coaxial Helicopter"):
                 xml_group.attrib["image"] = "HelicopterCoaxial"

Tools/px_romfs_pruner.py

@@ -1,8 +1,7 @@
 #!/usr/bin/env python
 ############################################################################
 #
-#   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+#   Copyright (C) 2014-2015 PX4 Development Team. All rights reserved.
-#   Author: Julian Oes <joes@student.ethz.ch>
 
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -37,48 +36,53 @@
 """
 px_romfs_pruner.py:
 Delete all comments and newlines before ROMFS is converted to an image
+
+@author: Julian Oes <julian@oes.ch>
 """
 
 from __future__ import print_function
-import argparse, re
+import argparse
+import re
 import os
 
 
 def main():
+    # Parse commandline arguments
+    parser = argparse.ArgumentParser(description="ROMFS pruner.")
+    parser.add_argument('--folder', action="store",
+                        help="ROMFS scratch folder.")
+    args = parser.parse_args()
 
-        # Parse commandline arguments
+    print("Pruning ROMFS files.")
-        parser = argparse.ArgumentParser(description="ROMFS pruner.")
-        parser.add_argument('--folder', action="store", help="ROMFS scratch folder.")
-        args = parser.parse_args()
 
-        print("Pruning ROMFS files.")
+    # go through
+    for (root, dirs, files) in os.walk(args.folder):
+        for file in files:
+            # only prune text files
+            if ".zip" in file or ".bin" in file or ".swp" in file \
+                    or ".data" in file or ".DS_Store" in file \
+                    or file.startswith("."):
+                continue
 
-        # go through
+            file_path = os.path.join(root, file)
-        for (root, dirs, files) in os.walk(args.folder):
-                for file in files:
-                        # only prune text files
-                        if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file or ".DS_Store" in file:
-                                continue
 
-                        file_path = os.path.join(root, file)
+            # read file line by line
-
+            pruned_content = ""
-                        # read file line by line
+            with open(file_path, "rU") as f:
-                        pruned_content = ""
+                for line in f:
-                        with open(file_path, "rU") as f:
+                    # handle mixer files differently than startup files
-                                for line in f:
+                    if file_path.endswith(".mix"):
-
+                        if line.startswith(("Z:", "M:", "R: ", "O:", "S:")):
-                                        # handle mixer files differently than startup files
+                                            pruned_content += line
-                                        if file_path.endswith(".mix"):
+                    else:
-                                                if line.startswith(("Z:", "M:", "R: ", "O:", "S:")):
+                        if not line.isspace() \
-                                                        pruned_content += line
+                                and not line.strip().startswith("#"):
-                                        else:
+                            pruned_content += line
-                                                if not line.isspace() and not line.strip().startswith("#"):
+            # overwrite old scratch file
-                                                        pruned_content += line
+            with open(file_path, "wb") as f:
-                       # overwrite old scratch file
+                pruned_content = re.sub("\r\n", "\n", pruned_content)
-                        with open(file_path, "wb") as f:
+                f.write(pruned_content.encode("ascii", errors='strict'))
-                                pruned_content = re.sub("\r\n", "\n", pruned_content)
-                                f.write(pruned_content.encode("ascii", errors='strict'))
 
 
 if __name__ == '__main__':
-        main()
+    main()

Tools/px_uploader.py

@@ -449,8 +449,9 @@ class uploader(object):
                 if self.board_type != fw.property('board_id'):
                         msg = "Firmware not suitable for this board (board_type=%u board_id=%u)" % (
                                 self.board_type, fw.property('board_id'))
+                        print("WARNING: %s" % msg)
                         if args.force:
-                                print("WARNING: %s" % msg)
+                                print("FORCED WRITE, FLASHING ANYWAY!")
                         else:
                                 raise IOError(msg)
                 if self.fw_maxsize < fw.property('image_size'):

cmake/configs/posix_rpi2_default.cmake

@@ -0,0 +1,47 @@
+include(posix/px4_impl_posix)
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake)
+
+set(config_module_list
+	drivers/device
+	platforms/common
+	platforms/posix/px4_layer
+	platforms/posix/work_queue
+	systemcmds/param
+	systemcmds/mixer
+	systemcmds/ver
+	systemcmds/esc_calib
+	systemcmds/reboot
+	systemcmds/topic_listener
+	systemcmds/perf
+	modules/uORB
+	modules/param
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/sensors
+	modules/mavlink
+	modules/attitude_estimator_q
+	modules/position_estimator_inav
+	modules/navigator
+	modules/vtol_att_control
+	modules/mc_pos_control
+	modules/mc_att_control
+	modules/land_detector
+	modules/fw_att_control
+	modules/fw_pos_control_l1
+	modules/dataman
+	modules/sdlog2
+	modules/commander
+	modules/controllib
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/conversion
+	lib/ecl
+	lib/external_lgpl
+	lib/geo
+	lib/geo_lookup
+	lib/launchdetection
+	lib/terrain_estimation
+	lib/runway_takeoff
+	lib/tailsitter_recovery
+)

cmake/configs/posix_rpi2_release.cmake

@@ -0,0 +1,56 @@
+include(posix/px4_impl_posix)
+
+if ("${RPI_TOOLCHAIN_DIR}" STREQUAL "")
+	set(RPI_TOOLCHAIN_DIR /opt/rpi_toolchain)
+endif()
+
+set(CMAKE_PROGRAM_PATH 
+	"${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin"
+	${CMAKE_PROGRAM_PATH}
+	)
+
+set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake)
+
+set(config_module_list
+	drivers/device
+	platforms/common
+	platforms/posix/px4_layer
+	platforms/posix/work_queue
+	systemcmds/param
+	systemcmds/mixer
+	systemcmds/ver
+	systemcmds/esc_calib
+	systemcmds/reboot
+	systemcmds/topic_listener
+	systemcmds/perf
+	modules/uORB
+	modules/param
+	modules/systemlib
+	modules/systemlib/mixer
+	modules/sensors
+	modules/mavlink
+	modules/attitude_estimator_q
+	modules/position_estimator_inav
+	modules/navigator
+	modules/vtol_att_control
+	modules/mc_pos_control
+	modules/mc_att_control
+	modules/land_detector
+	modules/fw_att_control
+	modules/fw_pos_control_l1
+	modules/dataman
+	modules/sdlog2
+	modules/commander
+	modules/controllib
+	lib/mathlib
+	lib/mathlib/math/filter
+	lib/conversion
+	lib/ecl
+	lib/external_lgpl
+	lib/geo
+	lib/geo_lookup
+	lib/launchdetection
+	lib/terrain_estimation
+	lib/runway_takeoff
+	lib/tailsitter_recovery
+)

cmake/configs/posix_sitl_default.cmake

@@ -34,6 +34,7 @@ set(config_module_list
 	modules/mavlink
 	modules/attitude_estimator_ekf
 	modules/attitude_estimator_q
+	modules/ekf2
 	modules/ekf_att_pos_estimator
 	modules/position_estimator_inav
 	modules/navigator

msg/ekf2_innovations.msg

@@ -0,0 +1,7 @@
+uint64 timestamp		# Timestamp in microseconds since boot
+float32[6] vel_pos_innov  # velocity and position innovations
+float32[3] mag_innov 	# earth magnetic field innovations
+float32 heading_innov 	# heading innovation
+float32[6] vel_pos_innov_var 	# velocity and position innovation variances
+float32[3] mag_innov_var	# earth magnetic field innovation variance
+float32 heading_innov_var 	# heading innovation variance

msg/manual_control_setpoint.msg

@@ -38,8 +38,9 @@ float32 aux5			 # default function: payload drop
 
 uint8 mode_switch		 # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
 uint8 return_switch		 # return to launch 2 position switch (mandatory): _NORMAL_, RTL
-uint8 rattitude_switch		 # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE
+uint8 rattitude_switch	 # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE
 uint8 posctl_switch		 # position control 2 position switch (optional): _ALTCTL_, POSCTL
 uint8 loiter_switch		 # loiter 2 position switch (optional): _MISSION_, LOITER
 uint8 acro_switch		 # acro 2 position switch (optional): _MANUAL_, ACRO
-uint8 offboard_switch		 # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
+uint8 offboard_switch	 # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
+uint8 kill_switch		 # throttle kill: _NORMAL_, KILL

msg/parameter_update.msg

@@ -1 +1,2 @@
 uint64 timestamp	# time at which the latest parameter was updated
+bool saved		# wether the change has already been saved to disk

msg/rc_channels.msg

@@ -1,4 +1,4 @@
-int32 RC_CHANNELS_FUNCTION_MAX=20
+int32 RC_CHANNELS_FUNCTION_MAX=21
 uint8 RC_CHANNELS_FUNCTION_THROTTLE=0
 uint8 RC_CHANNELS_FUNCTION_ROLL=1
 uint8 RC_CHANNELS_FUNCTION_PITCH=2
@@ -19,11 +19,12 @@ uint8 RC_CHANNELS_FUNCTION_PARAM_1=16
 uint8 RC_CHANNELS_FUNCTION_PARAM_2=17
 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
 uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19
+uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20
 uint64 timestamp						# Timestamp in microseconds since boot time
 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
-float32[20] channels						# Scaled to -1..1 (throttle: 0..1)
+float32[18] channels						# Scaled to -1..1 (throttle: 0..1)
 uint8 channel_count						# Number of valid channels
-int8[20] function						# Functions mapping
+int8[21] function						# Functions mapping
 uint8 rssi							# Receive signal strength index
 bool signal_lost						# Control signal lost, should be checked together with topic timeout
 uint32 frame_drop_count						# Number of dropped frames

nuttx-configs/px4io-v2/nsh/defconfig

@@ -164,8 +164,8 @@ CONFIG_USART1_SERIAL_CONSOLE=y
 CONFIG_USART2_SERIAL_CONSOLE=n
 CONFIG_USART3_SERIAL_CONSOLE=n
 
-CONFIG_USART1_TXBUFSIZE=64
+CONFIG_USART1_TXBUFSIZE=32
-CONFIG_USART2_TXBUFSIZE=64
+CONFIG_USART2_TXBUFSIZE=32
 CONFIG_USART3_TXBUFSIZE=64
 
 CONFIG_USART1_RXBUFSIZE=64

posix-configs/SITL/init/rcS_jmavsim_iris

@@ -51,8 +51,7 @@ sensors start
 commander start
 land_detector start multicopter
 navigator start
-attitude_estimator_q start
+ekf2 start
-position_estimator_inav start
 mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix

posix-configs/SITL/init/rcS_lpe_jmavsim_iris

@@ -3,8 +3,10 @@ simulator start -s
 param load
 param set MAV_TYPE 2
 param set MC_PITCHRATE_P 0.15
+param set MC_PITCH_P 7
+param set MC_ROLL_P 7
 param set MC_ROLLRATE_P 0.15
-param set MC_YAW_P 2.0
+param set MC_YAW_P 2.8
 param set MC_YAWRATE_P 0.35
 param set SYS_AUTOSTART 4010
 param set SYS_RESTART_TYPE 2
@@ -26,12 +28,22 @@ param set MPC_XY_P 0.4
 param set MPC_XY_VEL_P 0.2
 param set MPC_XY_VEL_D 0.005
 param set SENS_BOARD_ROT 0
+param set COM_RC_IN_MODE 1
+param set NAV_ACC_RAD 2.0
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 10.0
+param set MIS_TAKEOFF_ALT 5.0
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_HOLD_XY_DZ 0.1
+param set MPC_HOLD_Z_DZ 0.1
+param set MPC_Z_VEL_MAX 2.0
+param set MPC_Z_VEL_P 0.4
 
 # changes for LPE
 param set COM_RC_IN_MODE 1
 param set LPE_BETA_MAX 10000
 
-rgbled start
+rgbledsim start
 tone_alarm start
 gyrosim start
 accelsim start
@@ -49,6 +61,7 @@ mc_pos_control start
 mc_att_control start
 mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix
 mavlink start -u 14556 -r 2000000
+mavlink start -u 14557 -r 2000000 -m onboard
 mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556
 mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556
 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556
@@ -57,6 +70,7 @@ 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 stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
+mavlink stream -r 20 -s MANUAL_CONTROL -u 14556
 mavlink boot_complete
 sdlog2 start -r 100 -e -t -a
 

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

@@ -154,13 +154,12 @@ __BEGIN_DECLS
  *
  * 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)
+#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14)
 
 // 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
  *

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

@@ -219,10 +219,6 @@ __EXPORT int nsh_archinitialize(void)
 	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);
@@ -306,7 +302,7 @@ __EXPORT int nsh_archinitialize(void)
 	 */
 
 	// XXX start with 10.4 MHz and go up to 20 once validated
-	SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000);
+	SPI_SETFREQUENCY(spi2, 20 * 1000 * 1000);
 	SPI_SETBITS(spi2, 8);
 	SPI_SETMODE(spi2, SPIDEV_MODE3);
 	SPI_SELECT(spi2, SPIDEV_FLASH, false);

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

@@ -70,36 +70,36 @@ __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,
+		.default_value = 900,
 	},
 	{
 		.gpio = GPIO_TIM1_CH3OUT,
 		.timer_index = 0,
 		.timer_channel = 3,
-		.default_value = 1000,
+		.default_value = 900,
 	},
 	{
 		.gpio = GPIO_TIM1_CH2OUT,
 		.timer_index = 0,
 		.timer_channel = 2,
-		.default_value = 1000,
+		.default_value = 900,
 	},
 	{
 		.gpio = GPIO_TIM1_CH1OUT,
 		.timer_index = 0,
 		.timer_channel = 1,
-		.default_value = 1000,
+		.default_value = 900,
 	},
 	{
 		.gpio = GPIO_TIM4_CH2OUT,
 		.timer_index = 1,
 		.timer_channel = 2,
-		.default_value = 1000,
+		.default_value = 900,
 	},
 	{
 		.gpio = GPIO_TIM4_CH3OUT,
 		.timer_index = 1,
 		.timer_channel = 3,
-		.default_value = 1000,
+		.default_value = 900,
 	}
 };

src/drivers/device/spi.h

@@ -77,6 +77,15 @@ protected:
 	    int irq = 0);
 	virtual ~SPI();
 
+	/**
+	 * Locking modes supported by the driver.
+	 */
+	enum LockMode {
+		LOCK_PREEMPTION,	/**< the default; lock against all forms of preemption. */
+		LOCK_THREADS,		/**< lock only against other threads, using SPI_LOCK */
+		LOCK_NONE		/**< perform no locking, only safe if the bus is entirely private */
+	};
+
 	virtual int	init();
 
 	/**
@@ -117,13 +126,14 @@ protected:
 	void		set_frequency(uint32_t frequency);
 
 	/**
-	 * Locking modes supported by the driver.
+	 * Set the SPI bus locking mode
+	 *
+	 * This set the SPI locking mode. For devices competing with NuttX SPI
+	 * drivers on a bus the right lock mode is LOCK_THREADS.
+	 *
+	 * @param mode	Locking mode
 	 */
-	enum LockMode {
+	void		set_lockmode(enum LockMode mode) { locking_mode = mode; }
-		LOCK_PREEMPTION,	/**< the default; lock against all forms of preemption. */
-		LOCK_THREADS,		/**< lock only against other threads, using SPI_LOCK */
-		LOCK_NONE		/**< perform no locking, only safe if the bus is entirely private */
-	};
 
 	LockMode	locking_mode;	/**< selected locking mode */
 

src/drivers/frsky_telemetry/CMakeLists.txt

@@ -38,6 +38,7 @@ px4_add_module(
 		-Os
 	SRCS
 		frsky_data.c
+		sPort_data.c
 		frsky_telemetry.c
 	DEPENDS
 		platforms__common

src/drivers/frsky_telemetry/frsky_telemetry.c

@@ -35,11 +35,13 @@
 /**
  * @file frsky_telemetry.c
  * @author Stefan Rado <px4@sradonia.net>
+ * @author Mark Whitehorn <kd0aij@github.com>
  *
- * FrSky telemetry implementation.
+ * FrSky D8 mode and SmartPort (D16 mode) telemetry implementation.
  *
- * This daemon emulates an FrSky sensor hub by periodically sending data
+ * This daemon emulates the FrSky Sensor Hub for D8 mode.
- * packets to an attached FrSky receiver.
+ * For X series receivers (D16 mode) it emulates SmartPort sensors by responding to polling
+ * packets received from an attached FrSky X series receiver.
  *
  */
 
@@ -48,12 +50,15 @@
 #include <stdbool.h>
 #include <string.h>
 #include <sys/types.h>
+#include <poll.h>
 #include <fcntl.h>
 #include <unistd.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 #include <termios.h>
+#include <drivers/drv_hrt.h>
 
+#include "sPort_data.h"
 #include "frsky_data.h"
 
 
@@ -63,7 +68,8 @@ static volatile bool thread_running = false;
 static int frsky_task;
 
 /* functions */
-static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original);
+static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original);
+static int set_uart_speed(int uart, struct termios *uart_config, speed_t speed);
 static void usage(void);
 static int frsky_telemetry_thread_main(int argc, char *argv[]);
 __EXPORT int frsky_telemetry_main(int argc, char *argv[]);
@@ -72,10 +78,10 @@ __EXPORT int frsky_telemetry_main(int argc, char *argv[]);
 /**
  * Opens the UART device and sets all required serial parameters.
  */
-static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original)
+static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original)
 {
 	/* Open UART */
-	const int uart = open(uart_name, O_WRONLY | O_NOCTTY);
+	const int uart = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK);
 
 	if (uart < 0) {
 		err(1, "Error opening port: %s", uart_name);
@@ -91,22 +97,21 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
 	}
 
 	/* Fill the struct for the new configuration */
-	struct termios uart_config;
+	tcgetattr(uart, uart_config);
-	tcgetattr(uart, &uart_config);
 
 	/* Disable output post-processing */
-	uart_config.c_oflag &= ~OPOST;
+	uart_config->c_oflag &= ~OPOST;
 
 	/* Set baud rate */
-	static const speed_t speed = B9600;
+	static const speed_t speed = B57600;
 
-	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+	if (cfsetispeed(uart_config, speed) < 0 || cfsetospeed(uart_config, speed) < 0) {
 		warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
 
-	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
+	if ((termios_state = tcsetattr(uart, TCSANOW, uart_config)) < 0) {
 		warnx("ERR: %s (tcsetattr)\n", uart_name);
 		close(uart);
 		return -1;
@@ -115,6 +120,20 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
 	return uart;
 }
 
+static int set_uart_speed(int uart, struct termios *uart_config, speed_t speed)
+{
+
+	if (cfsetispeed(uart_config, speed) < 0) {
+		return -1;
+	}
+
+	if (tcsetattr(uart, TCSANOW, uart_config) < 0) {
+		return -1;
+	}
+
+	return uart;
+}
+
 /**
  * Print command usage information
  */
@@ -133,7 +152,7 @@ static void usage()
 static int frsky_telemetry_thread_main(int argc, char *argv[])
 {
 	/* Default values for arguments */
-	char *device_name = "/dev/ttyS1"; /* USART2 */
+	char *device_name = "/dev/ttyS6"; /* USART8 */
 
 	/* Work around some stupidity in task_create's argv handling */
 	argc -= 2;
@@ -154,42 +173,181 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 	}
 
 	/* Open UART */
+	warnx("opening uart");
 	struct termios uart_config_original;
-	const int uart = frsky_open_uart(device_name, &uart_config_original);
+	struct termios uart_config;
+	const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original);
 
 	if (uart < 0) {
+		warnx("could not open %s", device_name);
 		err(1, "could not open %s", device_name);
 	}
 
-	/* Subscribe to topics */
+	/* poll descriptor */
-	frsky_init();
+	struct pollfd fds[1];
+	fds[0].fd = uart;
+	fds[0].events = POLLIN;
 
 	thread_running = true;
 
 	/* Main thread loop */
-	unsigned int iteration = 0;
+	char sbuf[20];
 
-	while (!thread_should_exit) {
+	/* look for polling bytes indicating SmartPort telemetry
+	 * if not found, send D type telemetry frames instead
+	 */
+	int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 3000);
 
-		/* Sleep 200 ms */
+	if (status > 0) {
-		usleep(200000);
+		/* traffic on the port, assume it's a SmartPort master */
+		/* Subscribe to topics */
+		sPort_init();
 
-		/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
+		/* send S.port telemetry */
-		frsky_send_frame1(uart);
+		while (!thread_should_exit) {
 
-		/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */
+			/* wait for poll frame starting with value 0x7E
-		if (iteration % 5 == 0) {
+			* note that only the bus master is supposed to put a 0x7E on the bus.
-			frsky_send_frame2(uart);
+			* slaves use byte stuffing to send 0x7E and 0x7D.
+			* we expect a poll frame every 12msec
+			*/
+			int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50);
+
+			if (status < 1) { continue; }
+
+			// read 1 byte
+			int newBytes = read(uart, &sbuf[0], 1);
+
+			if (newBytes < 1 || sbuf[0] != 0x7E) { continue; }
+
+			/* wait for ID byte */
+			status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50);
+
+			if (status < 1) { continue; }
+
+			hrt_abstime now = hrt_absolute_time();
+
+			newBytes = read(uart, &sbuf[1], 1);
+
+			// allow a minimum of 500usec before reply
+			usleep(500);
+
+			static hrt_abstime lastBATV = 0;
+			static hrt_abstime lastCUR = 0;
+			static hrt_abstime lastALT = 0;
+			static hrt_abstime lastSPD = 0;
+			static hrt_abstime lastFUEL = 0;
+
+			switch (sbuf[1]) {
+
+			case SMARTPORT_POLL_1:
+
+				/* report BATV at 1Hz */
+				if (now - lastBATV > 1000 * 1000) {
+					lastBATV = now;
+					/* send battery voltage */
+					sPort_send_BATV(uart);
+				}
+
+				break;
+
+
+			case SMARTPORT_POLL_2:
+
+				/* report battery current at 5Hz */
+				if (now - lastCUR > 200 * 1000) {
+					lastCUR = now;
+					/* send battery current */
+					sPort_send_CUR(uart);
+				}
+
+				break;
+
+
+			case SMARTPORT_POLL_3:
+
+				/* report altitude at 5Hz */
+				if (now - lastALT > 200 * 1000) {
+					lastALT = now;
+					/* send altitude */
+					sPort_send_ALT(uart);
+				}
+
+				break;
+
+
+			case SMARTPORT_POLL_4:
+
+				/* report speed at 5Hz */
+				if (now - lastSPD > 200 * 1000) {
+					lastSPD = now;
+					/* send speed */
+					sPort_send_SPD(uart);
+				}
+
+				break;
+
+			case SMARTPORT_POLL_5:
+
+				/* report fuel at 1Hz */
+				if (now - lastFUEL > 1000 * 1000) {
+					lastFUEL = now;
+					/* send fuel */
+					sPort_send_FUEL(uart);
+				}
+
+				break;
+			}
 		}
 
-		/* Send frame 3 (every 5000ms): date, time */
+	} else {
-		if (iteration % 25 == 0) {
-			frsky_send_frame3(uart);
 
-			iteration = 0;
+		/* timed out: reconfigure UART and send D type telemetry */
+		warnx("SmartPort receiver not detected, sending FrSky D type telemetry");
+
+		status = set_uart_speed(uart, &uart_config, B9600);
+
+		if (status < 0) {
+			warnx("error setting speed for %s, quitting", device_name);
+			/* Reset the UART flags to original state */
+			tcsetattr(uart, TCSANOW, &uart_config_original);
+			close(uart);
+
+			thread_running = false;
+			return 0;
 		}
 
-		iteration++;
+		int iteration = 0;
+
+		/* Subscribe to topics */
+		frsky_init();
+
+		/* send D8 mode telemetry */
+		while (!thread_should_exit) {
+
+			/* Sleep 200 ms */
+			usleep(200000);
+
+			/* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */
+			frsky_send_frame1(uart);
+
+			/* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */
+			if (iteration % 5 == 0) {
+				frsky_send_frame2(uart);
+			}
+
+			/* Send frame 3 (every 5000ms): date, time */
+			if (iteration % 25 == 0) {
+				frsky_send_frame3(uart);
+
+				iteration = 0;
+			}
+
+			iteration++;
+		}
+
+		/* TODO: flush the input buffer if in full duplex mode */
+		read(uart, &sbuf[0], sizeof(sbuf));
 	}
 
 	/* Reset the UART flags to original state */
@@ -221,7 +379,7 @@ int frsky_telemetry_main(int argc, char *argv[])
 		thread_should_exit = false;
 		frsky_task = px4_task_spawn_cmd("frsky_telemetry",
 						SCHED_DEFAULT,
-						SCHED_PRIORITY_DEFAULT,
+						200,
 						2000,
 						frsky_telemetry_thread_main,
 						(char *const *)argv);

src/drivers/frsky_telemetry/sPort_data.c

@@ -0,0 +1,217 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Author: Stefan Rado <px4@sradonia.net>
+ *
+ * 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 sPort_data.c
+ * @author Stefan Rado <px4@sradonia.net>
+ * @author Mark Whitehorn <kd0aij@github.com>
+ *
+ * FrSky SmartPort telemetry implementation.
+ *
+ */
+
+#include "sPort_data.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <arch/math.h>
+#include <geo/geo.h>
+
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_status.h>
+
+#include <drivers/drv_hrt.h>
+
+#define frac(f) (f - (int)f)
+
+static int battery_sub = -1;
+static int sensor_sub = -1;
+static int global_position_sub = -1;
+static int vehicle_status_sub = -1;
+
+/**
+ * Initializes the uORB subscriptions.
+ */
+void sPort_init()
+{
+	battery_sub = orb_subscribe(ORB_ID(battery_status));
+	global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+}
+
+/**
+ * Sends a 0x10 start byte.
+ */
+//static void sPort_send_start(int uart)
+//{
+//	static const uint8_t c = 0x10;
+//	write(uart, &c, 1);
+//}
+
+static void update_crc(uint16_t *crc, unsigned char b)
+{
+	*crc += b;
+	*crc += *crc >> 8;
+	*crc &= 0xFF;
+}
+
+/**
+ * Sends one byte, performing byte-stuffing if necessary.
+ */
+static void sPort_send_byte(int uart, uint8_t value)
+{
+	const uint8_t x7E[] = { 0x7D, 0x5E };
+	const uint8_t x7D[] = { 0x7D, 0x5D };
+
+	switch (value) {
+	case 0x7E:
+		write(uart, x7E, sizeof(x7E));
+		break;
+
+	case 0x7D:
+		write(uart, x7D, sizeof(x7D));
+		break;
+
+	default:
+		write(uart, &value, sizeof(value));
+		break;
+	}
+}
+
+/**
+ * Sends one data id/value pair.
+ */
+void sPort_send_data(int uart, uint16_t id, uint32_t data)
+{
+	union {
+		uint32_t word;
+		uint8_t byte[4];
+	} buf;
+
+	/* send start byte */
+	static const uint8_t c = 0x10;
+	write(uart, &c, 1);
+
+	/* init crc */
+	uint16_t crc = c;
+
+	buf.word = id;
+
+	for (int i = 0; i < 2; i++) {
+		update_crc(&crc, buf.byte[i]);
+		sPort_send_byte(uart, buf.byte[i]);      /* LSB first */
+	}
+
+	buf.word = data;
+
+	for (int i = 0; i < 4; i++) {
+		update_crc(&crc, buf.byte[i]);
+		sPort_send_byte(uart, buf.byte[i]);      /* LSB first */
+	}
+
+	sPort_send_byte(uart, 0xFF - crc);
+}
+
+
+// scaling correct with OpenTX 2.1.7
+void sPort_send_BATV(int uart)
+{
+	/* get a local copy of the vehicle status data */
+	struct vehicle_status_s vehicle_status;
+	memset(&vehicle_status, 0, sizeof(vehicle_status));
+	orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
+
+	/* send battery voltage as VFAS */
+	uint32_t voltage = (int)(100 * vehicle_status.battery_voltage);
+	sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage);
+}
+
+// verified scaling
+void sPort_send_CUR(int uart)
+{
+	/* get a local copy of the vehicle status data */
+	struct vehicle_status_s vehicle_status;
+	memset(&vehicle_status, 0, sizeof(vehicle_status));
+	orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
+
+	/* send data */
+	uint32_t current = (int)(10 * vehicle_status.battery_current);
+	sPort_send_data(uart, SMARTPORT_ID_CURR, current);
+}
+
+// verified scaling for "custom" altitude option
+// OpenTX uses the initial reading as field elevation and displays
+// the difference (altitude - field)
+void sPort_send_ALT(int uart)
+{
+	/* get a local copy of the current sensor values */
+	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
+	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+
+	/* send data */
+	uint32_t alt = (int)(100 * raw.baro_alt_meter[0]);
+	sPort_send_data(uart, SMARTPORT_ID_ALT, alt);
+}
+
+// verified scaling for "calculated" option
+void sPort_send_SPD(int uart)
+{
+	/* get a local copy of the global position data */
+	struct vehicle_global_position_s global_pos;
+	memset(&global_pos, 0, sizeof(global_pos));
+	orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
+
+	/* send data for A2 */
+	float speed  = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
+	uint32_t ispeed = (int)(10 * speed);
+	sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
+}
+
+// verified scaling
+void sPort_send_FUEL(int uart)
+{
+	/* get a local copy of the vehicle status data */
+	struct vehicle_status_s vehicle_status;
+	memset(&vehicle_status, 0, sizeof(vehicle_status));
+	orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
+
+	/* send data */
+	uint32_t fuel = (int)(100 * vehicle_status.battery_remaining);
+	sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel);
+}

src/drivers/frsky_telemetry/sPort_data.h

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Author: Stefan Rado <px4@sradonia.net>
+ *
+ * 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 sPort_data.h
+ * @author Mark Whitehorn <kd0aij@github.com>
+ *
+ * FrSky SmartPort telemetry implementation.
+ *
+ */
+#ifndef _SPORT_DATA_H
+#define _SPORT_DATA_H
+
+#include <sys/types.h>
+
+/* FrSky SmartPort polling IDs captured from X4R */
+#define SMARTPORT_POLL_1    0x1B
+#define SMARTPORT_POLL_2    0x34
+#define SMARTPORT_POLL_3    0x95
+#define SMARTPORT_POLL_4    0x16
+#define SMARTPORT_POLL_5    0xB7
+
+/* FrSky SmartPort sensor IDs */
+#define SMARTPORT_ID_RSSI          0xf101
+#define SMARTPORT_ID_RXA1          0xf102	// supplied by RX
+#define SMARTPORT_ID_RXA2          0xf103	// supplied by RX
+#define SMARTPORT_ID_BATV          0xf104
+#define SMARTPORT_ID_SWR           0xf105
+#define SMARTPORT_ID_T1            0x0400
+#define SMARTPORT_ID_T2            0x0410
+#define SMARTPORT_ID_RPM           0x0500
+#define SMARTPORT_ID_FUEL          0x0600
+#define SMARTPORT_ID_ALT           0x0100
+#define SMARTPORT_ID_VARIO         0x0110
+#define SMARTPORT_ID_ACCX          0x0700
+#define SMARTPORT_ID_ACCY          0x0710
+#define SMARTPORT_ID_ACCZ          0x0720
+#define SMARTPORT_ID_CURR          0x0200
+#define SMARTPORT_ID_VFAS          0x0210
+#define SMARTPORT_ID_CELLS         0x0300
+#define SMARTPORT_ID_GPS_LON_LAT   0x0800
+#define SMARTPORT_ID_GPS_ALT       0x0820
+#define SMARTPORT_ID_GPS_SPD       0x0830
+#define SMARTPORT_ID_GPS_CRS       0x0840
+#define SMARTPORT_ID_GPS_TIME      0x0850
+
+// Public functions
+void sPort_init(void);
+void sPort_send_data(int uart, uint16_t id, uint32_t data);
+void sPort_send_BATV(int uart);
+void sPort_send_CUR(int uart);
+void sPort_send_ALT(int uart);
+void sPort_send_SPD(int uart);
+void sPort_send_FUEL(int uart);
+
+#endif /* _SPORT_TELEMETRY_H */

src/drivers/gps/gps.cpp

@@ -402,7 +402,12 @@ GPS::task_main()
 
 					if (!(_pub_blocked)) {
 						if (helper_ret & 1) {
-							orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
+							if (_report_gps_pos_pub != nullptr) {
+								orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
+
+							} else {
+								_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
+							}
 						}
 
 						if (_p_report_sat_info && (helper_ret & 2)) {

src/drivers/ms5611/ms5611_spi.cpp

@@ -131,7 +131,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum)
 }
 
 MS5611_SPI::MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
-	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */),
+	SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 20 * 1000 * 1000 /* will be rounded to 10.4 MHz */),
 	_prom(prom_buf)
 {
 }
@@ -152,6 +152,11 @@ MS5611_SPI::init()
 		goto out;
 	}
 
+	/* sharing a bus with NuttX drivers */
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
+	//set_lockmode(SPI::LOCK_THREADS);
+#endif
+
 	/* send reset command */
 	ret = _reset();
 

src/drivers/px4fmu/fmu.cpp

@@ -161,6 +161,8 @@ private:
 	volatile bool	_initialized;
 	bool		_throttle_armed;
 	bool		_pwm_on;
+	bool		_pwm_init;
+	uint8_t		_pwm_channel_mask;
 
 	MixerGroup	*_mixers;
 
@@ -313,6 +315,8 @@ PX4FMU::PX4FMU() :
 	_initialized(false),
 	_throttle_armed(false),
 	_pwm_on(false),
+	_pwm_init(false),
+	_pwm_channel_mask(0),
 	_mixers(nullptr),
 	_groups_required(0),
 	_groups_subscribed(0),
@@ -418,10 +422,7 @@ PX4FMU::set_mode(Mode mode)
 		_pwm_default_rate = 50;
 		_pwm_alt_rate = 50;
 		_pwm_alt_rate_channels = 0;
-
+		_pwm_channel_mask = 0x3;
-		/* XXX magic numbers */
-		up_pwm_servo_init(0x3);
-		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
 
 		break;
 
@@ -432,10 +433,7 @@ PX4FMU::set_mode(Mode mode)
 		_pwm_default_rate = 50;
 		_pwm_alt_rate = 50;
 		_pwm_alt_rate_channels = 0;
-
+		_pwm_channel_mask = 0xf;
-		/* XXX magic numbers */
-		up_pwm_servo_init(0xf);
-		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
 
 		break;
 
@@ -446,10 +444,7 @@ PX4FMU::set_mode(Mode mode)
 		_pwm_default_rate = 50;
 		_pwm_alt_rate = 50;
 		_pwm_alt_rate_channels = 0;
-
+		_pwm_channel_mask = 0x3f;
-		/* XXX magic numbers */
-		up_pwm_servo_init(0x3f);
-		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
 
 		break;
 
@@ -461,10 +456,7 @@ PX4FMU::set_mode(Mode mode)
 		_pwm_default_rate = 50;
 		_pwm_alt_rate = 50;
 		_pwm_alt_rate_channels = 0;
-
+		_pwm_channel_mask = 0xff;
-		/* XXX magic numbers */
-		up_pwm_servo_init(0xff);
-		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
 		break;
 #endif
 
@@ -477,6 +469,7 @@ PX4FMU::set_mode(Mode mode)
 
 		/* disable servo outputs - no need to set rates */
 		up_pwm_servo_deinit();
+		_pwm_init = false;
 
 		break;
 
@@ -873,6 +866,13 @@ PX4FMU::cycle()
 
 		if (_pwm_on != pwm_on) {
 			_pwm_on = pwm_on;
+
+			if (!_pwm_init && _pwm_on) {
+				up_pwm_servo_init(_pwm_channel_mask);
+				set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+				_pwm_init = true;
+			}
+
 			up_pwm_servo_arm(pwm_on);
 		}
 	}
@@ -1717,7 +1717,7 @@ ssize_t
 PX4FMU::write(file *filp, const char *buffer, size_t len)
 {
 	unsigned count = len / 2;
-	uint16_t values[6];
+	uint16_t values[8];
 
 #ifdef CONFIG_ARCH_BOARD_AEROCORE
 

src/drivers/px4io/px4io.cpp

@@ -273,6 +273,7 @@ private:
 	/* cached IO state */
 	uint16_t		_status;		///< Various IO status flags
 	uint16_t		_alarms;		///< Various IO alarms
+	uint16_t		_last_written_arming;	///< the last written arming state reg
 
 	/* subscribed topics */
 	int			_t_actuator_controls_0;	///< actuator controls group 0 topic
@@ -509,6 +510,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_perf_sample_latency(perf_alloc(PC_ELAPSED, "io latency")),
 	_status(0),
 	_alarms(0),
+	_last_written_arming(0),
 	_t_actuator_controls_0(-1),
 	_t_actuator_controls_1(-1),
 	_t_actuator_controls_2(-1),
@@ -977,7 +979,7 @@ PX4IO::task_main()
 		}
 
 		if (now >= poll_last + IO_POLL_INTERVAL) {
-			/* run at 50Hz */
+			/* run at 50-250Hz */
 			poll_last = now;
 
 			/* pull status and alarms from IO */
@@ -988,16 +990,6 @@ PX4IO::task_main()
 
 			/* fetch PWM outputs from IO */
 			io_publish_pwm_outputs();
-		}
-
-		if (now >= orb_check_last + ORB_CHECK_INTERVAL) {
-			/* run at 5Hz */
-			orb_check_last = now;
-
-			/* try to claim the MAVLink log FD */
-			if (_mavlink_fd < 0) {
-				_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
-			}
 
 			/* check updates on uORB topics and handle it */
 			bool updated = false;
@@ -1012,6 +1004,19 @@ PX4IO::task_main()
 			if (updated) {
 				io_set_arming_state();
 			}
+		}
+
+		if (now >= orb_check_last + ORB_CHECK_INTERVAL) {
+			/* run at 5Hz */
+			orb_check_last = now;
+
+			/* try to claim the MAVLink log FD */
+			if (_mavlink_fd < 0) {
+				_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
+			}
+
+			/* check updates on uORB topics and handle it */
+			bool updated = false;
 
 			/* vehicle command */
 			orb_check(_t_vehicle_command, &updated);
@@ -1350,7 +1355,15 @@ PX4IO::io_set_arming_state()
 		}
 	}
 
-	return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
+	uint16_t new_arming_state = set;
+	new_arming_state &= ~(clear);
+
+	if (_last_written_arming != new_arming_state) {
+		_last_written_arming = new_arming_state;
+		return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
+	}
+
+	return 0;
 }
 
 int

src/drivers/stm32/adc/adc.cpp

@@ -315,13 +315,13 @@ void
 ADC::update_system_power(void)
 {
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
-	system_power_s system_power;
+	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) {
+		if (_samples[i].am_channel == ADC_5V_RAIL_SENSE) {
 			// it is 2:1 scaled
 			system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096);
 		}
@@ -349,13 +349,13 @@ ADC::update_system_power(void)
 
 #endif // CONFIG_ARCH_BOARD_PX4FMU_V2
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V4
-	system_power_s system_power;
+	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) {
+		if (_samples[i].am_channel == ADC_5V_RAIL_SENSE) {
 			// it is 2:1 scaled
 			system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096);
 		}

src/lib/version/version.h

@@ -66,7 +66,12 @@
 #ifdef CONFIG_ARCH_BOARD_SITL
 #define	HW_ARCH "LINUXTEST"
 #endif
+
 #ifdef CONFIG_ARCH_BOARD_EAGLE
 #define	HW_ARCH "LINUXTEST"
 #endif
+
+#ifdef CONFIG_ARCH_BOARD_RPI2
+#define	HW_ARCH "LINUXTEST"
+#endif
 #endif /* VERSION_H_ */

src/modules/commander/accelerometer_calibration.cpp

@@ -595,7 +595,8 @@ int do_level_calibration(int mavlink_fd) {
 	param_get(pitch_offset_handle, &pitch_offset_current);
 	param_get(board_rot_handle, &board_rot_current);
 
-	if (board_rot_current == 0) {
+	// give attitude some time to settle if there have been changes to the board rotation parameters
+	if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON ) {
 		settle_time = 0;
 	}
 

src/modules/commander/commander.cpp

@@ -1509,12 +1509,12 @@ int commander_thread_main(int argc, char *argv[])
 			/* EPH / EPV */
 			param_get(_param_eph, &eph_threshold);
 			param_get(_param_epv, &epv_threshold);
-		}
 
-		/* Set flag to autosave parameters if necessary */
+			/* Set flag to autosave parameters if necessary */
-		if (updated && autosave_params != 0) {
+			if (updated && autosave_params != 0 && param_changed.saved == false) {
-			/* trigger an autosave */
+				/* trigger an autosave */
-			need_param_autosave = true;
+				need_param_autosave = true;
+			}
 		}
 
 		orb_check(sp_man_sub, &updated);
@@ -1570,11 +1570,12 @@ int commander_thread_main(int argc, char *argv[])
 				    /* and the system is not already armed (and potentially flying) */
 				    !armed.armed) {
 
-				    	bool chAirspeed = false;
+					bool chAirspeed = false;
-				    	bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
+					bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT;
 
 					/* Perform airspeed check only if circuit breaker is not
-					 * engaged and it's not a rotary wing */
+					 * engaged and it's not a rotary wing
+					 */
 					if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
 						chAirspeed = true;
 					}
@@ -2284,6 +2285,15 @@ int commander_thread_main(int argc, char *argv[])
 				mavlink_log_critical(mavlink_fd, "main state transition denied");
 			}
 
+			/* check throttle kill switch */
+			if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
+				/* set lockdown flag */
+				armed.lockdown = true;
+			} else if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
+				armed.lockdown = false;
+			}
+			/* no else case: do not change lockdown flag in unconfigured case */
+
 		} else {
 			if (!status.rc_input_blocked && !status.rc_signal_lost) {
 				mavlink_log_critical(mavlink_fd, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000);
@@ -2714,7 +2724,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 		}
 	}
 
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V1) || defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
 
 	/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
 	if (actuator_armed->armed) {

src/modules/commander/state_machine_helper.cpp

@@ -97,6 +97,9 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
 	"ARMING_STATE_IN_AIR_RESTORE",
 };
 
+static hrt_abstime last_preflight_check = 0;	///< initialize so it gets checked immediately
+static int last_prearm_ret = 1;			///< initialize to fail
+
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status,		///< current vehicle status
 			const struct safety_s   *safety,		///< current safety settings
@@ -132,12 +135,18 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 		}
 		/* re-run the pre-flight check as long as sensors are failing */
 		if (!status->condition_system_sensors_initialized 
-				&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED ||
+				&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
-				    new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
+				|| new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
-             			&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
+				&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
 
-            		prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */);
+			if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
-			status->condition_system_sensors_initialized = !prearm_ret;
+				prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */);
+				status->condition_system_sensors_initialized = !prearm_ret;
+				last_preflight_check = hrt_absolute_time();
+				last_prearm_ret = prearm_ret;
+			} else {
+				prearm_ret = last_prearm_ret;
+			}
 		}
 
 		/*

src/modules/ekf2/ekf2_main.cpp

@@ -72,7 +72,8 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/ekf2_innovations.h>
 
 #include <ecl/EKF/ekf.h>
 
@@ -84,7 +85,7 @@ class Ekf2;
 
 namespace ekf2
 {
-Ekf2 *instance;
+Ekf2 *instance = nullptr;
 }
 
 
@@ -116,6 +117,8 @@ public:
 
 	void print_status();
 
+	void exit() { _task_should_exit = true; }
+
 private:
 	static constexpr float _dt_max = 0.02;
 	bool		_task_should_exit = false;		/**< if true, task should exit */
@@ -124,11 +127,15 @@ private:
 	int		_sensors_sub = -1;
 	int		_gps_sub = -1;
 	int		_airspeed_sub = -1;
+	int		_params_sub = -1;
 
 	orb_advert_t _att_pub;
 	orb_advert_t _lpos_pub;
 	orb_advert_t _control_state_pub;
 	orb_advert_t _vehicle_global_position_pub;
+	orb_advert_t _estimator_status_pub;
+	orb_advert_t _estimator_innovations_pub;
+
 
 	/* Low pass filter for attitude rates */
 	math::LowPassFilter2p _lp_roll_rate;
@@ -162,24 +169,23 @@ private:
 
 	EstimatorBase *_ekf;
 
-
-	void update_parameters(bool force);
-
 	int update_subscriptions();
 
 };
 
 Ekf2::Ekf2():
 	SuperBlock(NULL, "EKF"),
+	_att_pub(nullptr),
+	_lpos_pub(nullptr),
+	_control_state_pub(nullptr),
+	_vehicle_global_position_pub(nullptr),
+	_estimator_status_pub(nullptr),
+	_estimator_innovations_pub(nullptr),
 	_lp_roll_rate(250.0f, 30.0f),
 	_lp_pitch_rate(250.0f, 30.0f),
-	_lp_yaw_rate(250.0f, 20.0f)
+	_lp_yaw_rate(250.0f, 20.0f),
+	_ekf(new Ekf())
 {
-	_ekf = new Ekf();
-	_att_pub = nullptr;
-	_lpos_pub = nullptr;
-	_control_state_pub = nullptr;
-
 	parameters *params = _ekf->getParamHandle();
 	_mag_delay_ms = new control::BlockParamFloat(this, "EKF2_MAG_DELAY", false, &params->mag_delay_ms);
 	_baro_delay_ms = new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, &params->baro_delay_ms);
@@ -230,34 +236,49 @@ void Ekf2::task_main()
 	_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 
-	px4_pollfd_struct_t fds[1];
+	px4_pollfd_struct_t fds[2] = {};
 	fds[0].fd = _sensors_sub;
 	fds[0].events = POLLIN;
+	fds[1].fd = _params_sub;
+	fds[1].events = POLLIN;
 
 	// initialise parameter cache
 	updateParams();
 
+	vehicle_gps_position_s gps = {};
+
 	while (!_task_should_exit) {
-		int ret = px4_poll(fds, 1, 1000);
+		int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000);
 
 		if (ret < 0) {
 			// Poll error, sleep and try again
 			usleep(10000);
 			continue;
 
-		} else if (ret == 0 || fds[0].revents != POLLIN) {
+		} else if (ret == 0) {
 			// Poll timeout or no new data, do nothing
 			continue;
 		}
 
-		updateParams();
+		if (fds[1].revents & POLLIN) {
+			// read from param to clear updated flag
+			struct parameter_update_s update;
+			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+			updateParams();
+
+			// fetch sensor data in next loop
+			continue;
+		} else if (!(fds[0].revents & POLLIN)) {
+			// no new data
+			continue;
+		}
 
 		bool gps_updated = false;
 		bool airspeed_updated = false;
 
 		sensor_combined_s sensors = {};
-		vehicle_gps_position_s gps = {};
 		airspeed_s airspeed = {};
 
 		orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
@@ -315,7 +336,7 @@ void Ekf2::task_main()
 		_ekf->update();
 
 		// generate vehicle attitude data
-		struct vehicle_attitude_s att;
+		struct vehicle_attitude_s att = {};
 		att.timestamp = hrt_absolute_time();
 
 		_ekf->copy_quaternion(att.q);
@@ -326,7 +347,7 @@ void Ekf2::task_main()
 		att.yaw = euler(2);
 
 		// generate vehicle local position data
-		struct vehicle_local_position_s lpos;
+		struct vehicle_local_position_s lpos = {};
 		float pos[3] = {};
 		float vel[3] = {};
 
@@ -355,8 +376,8 @@ void Ekf2::task_main()
 		lpos.xy_global =
 			_ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
 		lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt)
-		lpos.ref_lat = _ekf->_posRef.lat_rad * (double)180.0 * M_PI; // Reference point latitude in degrees
+		lpos.ref_lat = _ekf->_posRef.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees
-		lpos.ref_lon = _ekf->_posRef.lon_rad * (double)180.0 * M_PI; // Reference point longitude in degrees
+		lpos.ref_lon = _ekf->_posRef.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
 		lpos.ref_alt =
 			_ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level
 
@@ -421,7 +442,7 @@ void Ekf2::task_main()
 		}
 
 		// generate and publish global position data
-		struct vehicle_global_position_s global_pos;
+		struct vehicle_global_position_s global_pos = {};
 
 		if (_ekf->position_is_valid()) {
 			// TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator
@@ -460,7 +481,42 @@ void Ekf2::task_main()
 				orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
 			}
 		}
+
+		// publish estimator status
+		struct estimator_status_s status = {};
+		status.timestamp = hrt_absolute_time();
+		_ekf->get_state_delayed(status.states);
+		_ekf->get_covariances(status.covariances);
+
+		if (_estimator_status_pub == nullptr) {
+			_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status);
+
+		} else {
+			orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status);
+		}
+
+		// publish estimator innovation data
+		struct ekf2_innovations_s innovations = {};
+		innovations.timestamp = hrt_absolute_time();
+		_ekf->get_vel_pos_innov(&innovations.vel_pos_innov[0]);
+		_ekf->get_mag_innov(&innovations.mag_innov[0]);
+		_ekf->get_heading_innov(&innovations.heading_innov);
+
+		_ekf->get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
+		_ekf->get_mag_innov_var(&innovations.mag_innov_var[0]);
+		_ekf->get_heading_innov_var(&innovations.heading_innov_var);
+
+		if (_estimator_innovations_pub == nullptr) {
+			_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);
+
+		} else {
+			orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
+		}
+
 	}
+
+	delete ekf2::instance;
+	ekf2::instance = nullptr;
 }
 
 void Ekf2::task_main_trampoline(int argc, char *argv[])
@@ -525,8 +581,13 @@ int ekf2_main(int argc, char *argv[])
 			return 1;
 		}
 
-		delete ekf2::instance;
+		ekf2::instance->exit();
-		ekf2::instance = nullptr;
+
+		// wait for the destruction of the instance
+		while (ekf2::instance != nullptr) {
+			usleep(50000);
+		}
+
 		return 0;
 	}
 

src/modules/mavlink/CMakeLists.txt

@@ -41,7 +41,7 @@ px4_add_module(
 		${MODULE_CFLAGS}
 		-Wno-attributes
 		-Wno-packed
-		-DMAVLINK_COMM_NUM_BUFFERS=3
+		-DMAVLINK_COMM_NUM_BUFFERS=4
 		-Wno-packed
 		-Wno-tautological-constant-out-of-range-compare
 		-Os

src/modules/mavlink/mavlink_main.cpp

@@ -169,6 +169,7 @@ Mavlink::Mavlink() :
 	_flow_control_enabled(true),
 	_last_write_success_time(0),
 	_last_write_try_time(0),
+	_mavlink_start_time(0),
 	_bytes_tx(0),
 	_bytes_txerr(0),
 	_bytes_rx(0),
@@ -886,6 +887,10 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 
 	_last_write_try_time = hrt_absolute_time();
 
+	if (_mavlink_start_time == 0) {
+		_mavlink_start_time = _last_write_try_time;
+	}
+
 	if (get_protocol() == SERIAL) {
 		/* check if there is space in the buffer, let it overflow else */
 		unsigned buf_free = get_free_tx_buf();
@@ -938,6 +943,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 
 		/* resend heartbeat via broadcast */
 		if ((_mode != MAVLINK_MODE_ONBOARD)
+			&& (_mavlink_start_time > 0 && (hrt_elapsed_time(&_mavlink_start_time) > 4 * 1000 * 1000))
 			&& (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) ||
 			(tstatus.heartbeat_time == 0)) &&
 			msgid == MAVLINK_MSG_ID_HEARTBEAT)) {
@@ -1049,10 +1055,10 @@ Mavlink::init_udp()
 	/* set default target address, but not for onboard mode (will be set on first received packet) */
 	memset((char *)&_src_addr, 0, sizeof(_src_addr));
 	if (_mode != MAVLINK_MODE_ONBOARD) {
-		set_client_source_initialized();
 		_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);
+		set_client_source_initialized();
 	}
 
 	/* default broadcast address */
@@ -1796,7 +1802,6 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("GLOBAL_POSITION_INT", 10.0f);
 		configure_stream("HOME_POSITION", 0.5f);
 		configure_stream("ATTITUDE_TARGET", 10.0f);
-		configure_stream("BATTERY_STATUS", 1.0f);
 		configure_stream("SYSTEM_TIME", 1.0f);
 		configure_stream("RC_CHANNELS", 5.0f);
 		configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
@@ -2141,7 +2146,7 @@ Mavlink::start(int argc, char *argv[])
 	px4_task_spawn_cmd(buf,
 			   SCHED_DEFAULT,
 			   SCHED_PRIORITY_DEFAULT,
-			   2400,
+			   2700,
 			   (px4_main_t)&Mavlink::start_helper,
 			   (char *const *)argv);
 

src/modules/mavlink/mavlink_main.h

@@ -339,7 +339,7 @@ public:
 
 	int 			get_socket_fd () { return _socket_fd; };
 #ifdef __PX4_POSIX
-	struct sockaddr_in * get_client_source_address() {return &_src_addr;};
+	struct sockaddr_in *	get_client_source_address() {return &_src_addr;};
 
 	void			set_client_source_initialized() { _src_addr_initialized = true; };
 
@@ -416,6 +416,7 @@ private:
 	bool			_flow_control_enabled;
 	uint64_t		_last_write_success_time;
 	uint64_t		_last_write_try_time;
+	uint64_t		_mavlink_start_time;
 
 	unsigned		_bytes_tx;
 	unsigned		_bytes_txerr;

src/modules/mavlink/mavlink_messages.cpp

@@ -885,6 +885,9 @@ private:
 	MavlinkOrbSubscription *_airspeed_sub;
 	uint64_t _airspeed_time;
 
+	MavlinkOrbSubscription *_sensor_sub;
+	uint64_t _sensor_time;
+
 	/* do not allow top copying this class */
 	MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
 	MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
@@ -900,16 +903,18 @@ protected:
 		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
 		_act_time(0),
 		_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
-		_airspeed_time(0)
+		_airspeed_time(0),
+		_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
+		_sensor_time(0)
 	{}
 
 	void send(const hrt_abstime t)
 	{
-		struct vehicle_attitude_s att;
+		struct vehicle_attitude_s att = {};
-		struct vehicle_global_position_s pos;
+		struct vehicle_global_position_s pos = {};
-		struct actuator_armed_s armed;
+		struct actuator_armed_s armed = {};
-		struct actuator_controls_s act;
+		struct actuator_controls_s act = {};
-		struct airspeed_s airspeed;
+		struct airspeed_s airspeed = {};
 
 		bool updated = _att_sub->update(&_att_time, &att);
 		updated |= _pos_sub->update(&_pos_time, &pos);
@@ -924,7 +929,15 @@ protected:
 			msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
 			msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
 			msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
-			msg.alt = pos.alt;
+			if (_pos_time > 0) {
+				/* use global estimate */
+				msg.alt = pos.alt;
+			} else {
+				/* fall back to baro altitude */
+				sensor_combined_s sensor;
+				(void)_sensor_sub->update(&_sensor_time, &sensor);
+				msg.alt = sensor.baro_alt_meter[0];
+			}
 			msg.climb = -pos.vel_d;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
@@ -2668,6 +2681,9 @@ private:
 	MavlinkOrbSubscription *_home_sub;
 	uint64_t _home_time;
 
+	MavlinkOrbSubscription *_sensor_sub;
+	uint64_t _sensor_time;
+
 	/* do not allow top copying this class */
 	MavlinkStreamAltitude(MavlinkStreamAltitude &);
 	MavlinkStreamAltitude& operator = (const MavlinkStreamAltitude &);
@@ -2679,7 +2695,9 @@ protected:
 		_local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))),
 		_local_pos_time(0),
 		_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))),
-		_home_time(0)
+		_home_time(0),
+		_sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
+		_sensor_time(0)
 	{}
 
 	void send(const hrt_abstime t)
@@ -2693,21 +2711,25 @@ protected:
 		updated |= _home_sub->update(&_home_time, &home);
 
 		if (updated) {
+
+			struct sensor_combined_s sensor;
+			(void)_sensor_sub->update(&_sensor_time, &sensor);
+
 			mavlink_altitude_t msg;
 
 			msg.time_usec = hrt_absolute_time();
 
-			msg.altitude_monotonic = global_pos.pressure_alt;
+			msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter[0] : 0.0f / 0.0f;
-			msg.altitude_amsl = global_pos.alt;
+			msg.altitude_amsl = (_global_pos_time > 0) ? global_pos.alt : 0.0f / 0.0f;
-			msg.altitude_local = -local_pos.z;
+			msg.altitude_local = (_local_pos_time > 0) ? -local_pos.z : 0.0f / 0.0f;
-			msg.altitude_relative = home.alt;
+			msg.altitude_relative = (_home_time > 0) ? home.alt : 0.0f / 0.0f;
 
 			if (global_pos.terrain_alt_valid) {
 				msg.altitude_terrain = global_pos.terrain_alt;
 				msg.bottom_clearance = global_pos.alt - global_pos.terrain_alt;
 			} else {
-				msg.altitude_terrain = -1001;
+				msg.altitude_terrain = 0.0f / 0.0f;
-				msg.bottom_clearance = -1;
+				msg.bottom_clearance = 0.0f / 0.0f;
 			}
 
 			_mavlink->send_message(MAVLINK_MSG_ID_ALTITUDE, &msg);

src/modules/mavlink/mavlink_receiver.cpp

@@ -1771,7 +1771,7 @@ MavlinkReceiver::receive_thread(void *arg)
 		fds[0].events = POLLIN;
 	}
 #ifdef __PX4_POSIX
-	struct sockaddr_in srcaddr;
+	struct sockaddr_in srcaddr = {};
 	socklen_t addrlen = sizeof(srcaddr);
 
 	if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) {
@@ -1817,8 +1817,12 @@ MavlinkReceiver::receive_thread(void *arg)
 			if ((srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost))
 					|| (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_ONBOARD && !_mavlink->get_client_source_initialized())) {
 				// if we were sending to localhost before but have a new host then accept him
-				memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr));
+// This is causing issues on Linux, so use default port for now
-				PX4_WARN("UDP source addr changed: %s", inet_ntoa(srcaddr.sin_addr));
+// this will kill tablet testing on Linux and VMs
+#ifndef __PX4_LINUX
+				srcaddr_last->sin_addr.s_addr = srcaddr.sin_addr.s_addr;
+				srcaddr_last->sin_port = srcaddr.sin_port;
+#endif
 				_mavlink->set_client_source_initialized();
 			}
 #endif

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -199,6 +199,7 @@ private:
 		param_t vtol_type;
 		param_t roll_tc;
 		param_t pitch_tc;
+		param_t vtol_opt_recovery_enabled;
 
 	}		_params_handles;		/**< handles for interesting parameters */
 
@@ -218,6 +219,7 @@ private:
 		math::Vector<3> acro_rate_max;		/**< max attitude rates in acro mode */
 		float rattitude_thres;
 		int vtol_type;						/**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */
+		bool vtol_opt_recovery_enabled;
 	}		_params;
 
 	TailsitterRecovery *_ts_opt_recovery;	/**< Computes optimal rates for tailsitter recovery */
@@ -347,6 +349,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.mc_rate_max.zero();
 	_params.acro_rate_max.zero();
 	_params.rattitude_thres = 1.0f;
+	_params.vtol_opt_recovery_enabled = false;
 
 	_rates_prev.zero();
 	_rates_sp.zero();
@@ -383,11 +386,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.vtol_type 		= 	param_find("VT_TYPE");
 	_params_handles.roll_tc			= 	param_find("MC_ROLL_TC");
 	_params_handles.pitch_tc		= 	param_find("MC_PITCH_TC");
+	_params_handles.vtol_opt_recovery_enabled = param_find("VT_OPT_RECOV_EN");
 
 	/* fetch initial parameter values */
 	parameters_update();
 
-	if (_params.vtol_type == 0) {
+	if (_params.vtol_type == 0 && _params.vtol_opt_recovery_enabled) {
 		// the vehicle is a tailsitter, use optimal recovery control strategy
 		_ts_opt_recovery = new TailsitterRecovery();
 	}
@@ -415,8 +419,10 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl()
 			}
 		} while (_control_task != -1);
 	}
+	if (_ts_opt_recovery != nullptr) {
+		delete _ts_opt_recovery;
+	}
 
-	delete _ts_opt_recovery;
 	mc_att_control::g_control = nullptr;
 }
 
@@ -489,6 +495,10 @@ MulticopterAttitudeControl::parameters_update()
 
 	param_get(_params_handles.vtol_type, &_params.vtol_type);
 
+	int tmp;
+	param_get(_params_handles.vtol_opt_recovery_enabled, &tmp);
+	_params.vtol_opt_recovery_enabled = (bool)tmp;
+
 	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
 
 	return OK;

src/modules/mc_att_control/mc_att_control_params.c

@@ -254,13 +254,14 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 220.0f);
  * Limit for yaw rate, has effect for large rotations in autonomous mode,
  * to avoid large control output and mixer saturation. A value of significantly
  * over 60 degrees per second can already lead to mixer saturation.
+ * A value of 30 degrees / second is recommended to avoid very audible twitches.
  *
  * @unit deg/s
  * @min 0.0
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 60.0f);
+PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 45.0f);
 
 /**
  * Max acro roll rate

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -180,12 +180,12 @@ private:
 		param_t xy_ff;
 		param_t tilt_max_air;
 		param_t land_speed;
-		param_t tko_jmpspd;
 		param_t tko_speed;
 		param_t tilt_max_land;
 		param_t man_roll_max;
 		param_t man_pitch_max;
 		param_t man_yaw_max;
+		param_t global_yaw_max;
 		param_t mc_att_yaw_p;
 		param_t hold_xy_dz;
 		param_t hold_z_dz;
@@ -199,12 +199,12 @@ private:
 		float thr_max;
 		float tilt_max_air;
 		float land_speed;
-		float tko_jmpspd;
 		float tko_speed;
 		float tilt_max_land;
 		float man_roll_max;
 		float man_pitch_max;
 		float man_yaw_max;
+		float global_yaw_max;
 		float mc_att_yaw_p;
 		float hold_xy_dz;
 		float hold_z_dz;
@@ -245,11 +245,11 @@ private:
 
 	math::Matrix<3, 3> _R;			/**< rotation matrix from attitude quaternions */
 	float _yaw;				/**< yaw angle (euler) */
-	float _landing_thrust;
+	bool _in_landing;
-	hrt_abstime _landing_started;
 	bool _takeoff_jumped;
 	float _vel_z_lp;
 	float _acc_z_lp;
+	float _takeoff_thrust_sp;
 
 	/**
 	 * Update our local parameter cache.
@@ -374,11 +374,11 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_run_pos_control(true),
 	_run_alt_control(true),
 	_yaw(0.0f),
-	_landing_thrust(1.0f),
+	_in_landing(false),
-	_landing_started(0),
 	_takeoff_jumped(false),
 	_vel_z_lp(0),
-	_acc_z_lp(0)
+	_acc_z_lp(0),
+	_takeoff_thrust_sp(0.0f)
 {
 	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
 	memset(&_ctrl_state, 0, sizeof(_ctrl_state));
@@ -429,12 +429,12 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_handles.xy_ff		= param_find("MPC_XY_FF");
 	_params_handles.tilt_max_air	= param_find("MPC_TILTMAX_AIR");
 	_params_handles.land_speed	= param_find("MPC_LAND_SPEED");
-	_params_handles.tko_jmpspd	= param_find("MPC_TKO_JMPSPD");
 	_params_handles.tko_speed	= param_find("MPC_TKO_SPEED");
 	_params_handles.tilt_max_land	= param_find("MPC_TILTMAX_LND");
 	_params_handles.man_roll_max = param_find("MPC_MAN_R_MAX");
 	_params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX");
 	_params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX");
+	_params_handles.global_yaw_max = param_find("MC_YAWRATE_MAX");
 	_params_handles.mc_att_yaw_p = param_find("MC_YAW_P");
 	_params_handles.hold_xy_dz = param_find("MPC_HOLD_XY_DZ");
 	_params_handles.hold_z_dz = param_find("MPC_HOLD_Z_DZ");
@@ -493,7 +493,6 @@ MulticopterPositionControl::parameters_update(bool force)
 		param_get(_params_handles.tilt_max_air, &_params.tilt_max_air);
 		_params.tilt_max_air = math::radians(_params.tilt_max_air);
 		param_get(_params_handles.land_speed, &_params.land_speed);
-		param_get(_params_handles.tko_jmpspd, &_params.tko_jmpspd);
 		param_get(_params_handles.tko_speed, &_params.tko_speed);
 		param_get(_params_handles.tilt_max_land, &_params.tilt_max_land);
 		_params.tilt_max_land = math::radians(_params.tilt_max_land);
@@ -551,9 +550,12 @@ MulticopterPositionControl::parameters_update(bool force)
 		param_get(_params_handles.man_roll_max, &_params.man_roll_max);
 		param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
 		param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
+		param_get(_params_handles.global_yaw_max, &_params.global_yaw_max);
 		_params.man_roll_max = math::radians(_params.man_roll_max);
 		_params.man_pitch_max = math::radians(_params.man_pitch_max);
 		_params.man_yaw_max = math::radians(_params.man_yaw_max);
+		_params.global_yaw_max = math::radians(_params.global_yaw_max);
+
 		param_get(_params_handles.mc_att_yaw_p, &v);
 		_params.mc_att_yaw_p = v;
 	}
@@ -1358,25 +1360,36 @@ MulticopterPositionControl::task_main()
 				if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
 				    && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 
+					// check if we are not already in air.
+					// if yes then we don't need a jumped takeoff anymore
+					if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON ) {
+						_takeoff_jumped = true;
+					}
+
 					if (!_takeoff_jumped) {
-						/* do a quick jump (ramp vel sp up in 1/2sec) until we shoot over takeoff speed */
+						// ramp thrust setpoint up
-						float accel = _vel_sp_prev(2) - _params.tko_jmpspd;
+						if (_vel(2) > -(_params.tko_speed / 2.0f)) {
-						float tmp = _vel_sp_prev(2) + accel * dt * 2.0f;
+							_takeoff_thrust_sp += 0.5f * dt;
-						_vel_sp(2) = math::max(tmp, -_params.tko_jmpspd);
+							_vel_sp.zero();
-
+							_vel_prev.zero();
-						if (_vel(2) < -_params.tko_speed) {
+						} else {
+							// copter has reached our takeoff speed. split the thrust setpoint up
+							// into an integral part and into a P part
+							thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2));
+							thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max);
+							_vel_sp_prev(2) = -_params.tko_speed;
 							_takeoff_jumped = true;
+							reset_int_z = false;
 						}
+					}
 
-					} else {
+					if (_takeoff_jumped) {
-						/* slowly reduce forced takeoff speed to set climb rate (in 3 sec) */
+						_vel_sp(2) = -_params.tko_speed;
-						float decel = _params.tko_jmpspd - _params.tko_speed;
-						float tmp = decel / 3.0f * dt + _vel_sp_prev(2);
-						_vel_sp(2) = math::min(tmp, -_params.tko_speed);
 					}
 
 				} else {
 					_takeoff_jumped = false;
+					_takeoff_thrust_sp = 0.0f;
 				}
 
 				// limit total horizontal acceleration
@@ -1393,6 +1406,14 @@ MulticopterPositionControl::task_main()
 					_vel_sp(1) = vel_sp_hor(1);
 				}
 
+				// limit vertical acceleration
+				float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
+
+				if (fabsf(acc_v) > 2 * _params.acc_hor_max) {
+					acc_v /= fabsf(acc_v);
+					_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);
+				}
+
 				_vel_sp_prev = _vel_sp;
 
 				_global_vel_sp.vx = _vel_sp(0);
@@ -1450,6 +1471,13 @@ MulticopterPositionControl::task_main()
 					// TODO?: + _vel_sp.emult(_params.vel_ff)
 					math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;
 
+					if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
+							&& !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) {
+						// for jumped takeoffs use special thrust setpoint calculated above
+						thrust_sp.zero();
+						thrust_sp(2) = -_takeoff_thrust_sp;
+					}
+
 					if (!_control_mode.flag_control_velocity_enabled) {
 						thrust_sp(0) = 0.0f;
 						thrust_sp(1) = 0.0f;
@@ -1490,48 +1518,32 @@ MulticopterPositionControl::task_main()
 							thr_min = 0.0f;
 						}
 
-						if (_landing_started == 0) {
+						/* descent stabilized, we're in landing */
-							_landing_started = hrt_absolute_time();
+						if (!_in_landing
-						}
-
-						#if 0
-						// TODO quick fix: remove this since in combination with the non-working fall detection
-						// it can lead to the copter falling out of the sky
-						/* don't let it throttle up again during landing */
-						if (thrust_sp(2) < 0.0f && thrust_abs < _landing_thrust
-								/* fix landing thrust after a certain time when velocity change is minimal */
 								&& (float)fabs(_acc_z_lp) < 0.1f
-								&& _vel_z_lp > 0.5f * _params.land_speed
+								&& _vel_z_lp > 0.5f * _params.land_speed) {
-								&& hrt_elapsed_time(&_landing_started) > 15e5) {
+							_in_landing = true;
-							_landing_thrust = thrust_abs;
 						}
-						#endif
 
 						/* assume ground, reduce thrust */
-						if (hrt_elapsed_time(&_landing_started) > 15e5
+						if (_in_landing
-								&& _landing_thrust > FLT_EPSILON
 								&& _vel_z_lp < 0.1f
 								) {
-							_landing_thrust = 0.0f;
+							thr_max = 0.0f;
 						}
 
 						/* if we suddenly fall, reset landing logic and remove thrust limit */
-						if (hrt_elapsed_time(&_landing_started) > 15e5
+						if (_in_landing
-								&& _landing_thrust < FLT_EPSILON
 								/* XXX: magic value, assuming free fall above 4m/s2 acceleration */
 								&& (_acc_z_lp > 4.0f
-								|| _vel_z_lp > 2.0f * _params.land_speed)
+									|| _vel_z_lp > 2.0f * _params.land_speed)
 								) {
-							_landing_thrust = _params.thr_max;
+							thr_max = _params.thr_max;
-							_landing_started = 0;
+							_in_landing = false;
 						}
 
-						/* add 5% to give it some margin to compensate external influences */
-						thr_max = _landing_thrust + 0.05f * _landing_thrust;
-
 					} else {
-						_landing_started = 0;
+						_in_landing = false;
-						_landing_thrust = _params.thr_max;
 					}
 
 					/* limit min lift */
@@ -1766,14 +1778,19 @@ MulticopterPositionControl::task_main()
 			/* do not move yaw while sitting on the ground */
 			else if (!_vehicle_status.condition_landed &&
 				!(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) {
-				const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p;
 
-				_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
+				/* we want to know the real constraint, and global overrides manual */
+				const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max : _params.global_yaw_max;
+				const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p;
+
+				_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;
 				float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
 				float yaw_offs = _wrap_pi(yaw_target - _yaw);
 
 				// If the yaw offset became too big for the system to track stop
 				// shifting it
+
+				// XXX this needs inspection - probably requires a clamp, not an if
 				if (fabsf(yaw_offs) < yaw_offset_max) {
 					_att_sp.yaw_body = yaw_target;
 				}

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -234,15 +234,6 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f);
  */
 PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.5f);
 
-/**
- * Takeoff initial speed to jump off the ground
- *
- * @unit m/s
- * @min 0.0
- * @group Multicopter Position Control
- */
-PARAM_DEFINE_FLOAT(MPC_TKO_JMPSPD, 2.0f);
-
 /**
  * Takeoff climb rate
  *

src/modules/navigator/mission.cpp

@@ -188,11 +188,11 @@ Mission::on_active()
 		}
 	}
 
-	/* see if we need to update the current yaw heading for rotary wing types */
+	/* see if we need to update the current yaw heading */
-	if (_navigator->get_vstatus()->is_rotary_wing
+	if ((_param_yawmode.get() != MISSION_YAWMODE_NONE
-			&& _param_yawmode.get() != MISSION_YAWMODE_NONE
 			&& _param_yawmode.get() < MISSION_YAWMODE_MAX
-			&& _mission_type != MISSION_TYPE_NONE) {
+			&& _mission_type != MISSION_TYPE_NONE)
+			|| _navigator->get_vstatus()->is_vtol) {
 		heading_sp_update();
 	}
 
@@ -259,7 +259,8 @@ Mission::update_offboard_mission()
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
 				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
 				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-				_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
+				_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius(),
+				_navigator->get_vstatus()->condition_landed);
 
 		_navigator->get_mission_result()->valid = !failed;
 		_navigator->increment_mission_instance_count();
@@ -561,7 +562,8 @@ Mission::heading_sp_update()
 		}
 
 		/* always keep the front of the rotary wing pointing to the next waypoint */
-		if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) {
+		if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT
+			|| _navigator->get_vstatus()->is_vtol) {
 			_mission_item.yaw = get_bearing_to_next_waypoint(
 				point_from_latlon[0],
 				point_from_latlon[1],
@@ -843,7 +845,8 @@ Mission::check_mission_valid()
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
 				_navigator->get_home_position()->alt, _navigator->home_position_valid(),
 				_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-				_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
+				_param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius(),
+				_navigator->get_vstatus()->condition_landed);
 
 		_navigator->increment_mission_instance_count();
 		_navigator->set_mission_result_updated();

src/modules/navigator/mission_feasibility_checker.cpp

@@ -64,7 +64,9 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() :
 
 bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing,
 	dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
-	float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued)
+	float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued,
+	float default_acceptance_rad,
+	bool condition_landed)
 {
 	bool failed = false;
 	bool warned = false;
@@ -83,12 +85,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota
 	}
 
 	// check if all mission item commands are supported
-	failed = failed || !checkMissionItemValidity(dm_current, nMissionItems);
+	failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, condition_landed);
 	failed = failed || !checkGeofence(dm_current, nMissionItems, geofence);
 	failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);
 
 	if (isRotarywing) {
-		failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
+		failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad);
 	} else {
 		failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
 	}
@@ -96,9 +98,41 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota
 	return !failed;
 }
 
-bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
+bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems,
+	Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad)
 {
-	/* no custom rotary wing checks yet */
+	/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
+	for (size_t i = 0; i < nMissionItems; i++) {
+		struct mission_item_s missionitem;
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		if (dm_read(dm_current, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return false;
+		}
+
+		// look for a takeoff waypoint
+		if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) {
+			// make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius
+			// this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air
+			float takeoff_alt = missionitem.altitude_is_relative
+				      ? missionitem.altitude
+			              : missionitem.altitude - home_alt;
+			// check if we should use default acceptance radius
+			float acceptance_radius = default_acceptance_rad;
+
+			if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) {
+				acceptance_radius = missionitem.acceptance_radius;
+			}
+
+			if (takeoff_alt - 1.0f < acceptance_radius) {
+				mavlink_log_critical(_mavlink_fd, "Mission rejected: Takeoff altitude too low!");
+				return false;
+			}
+		}
+	}
+
+	// all checks have passed
 	return true;
 }
 
@@ -178,7 +212,7 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 	return true;
 }
 
-bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems) {
+bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed) {
 	// do not allow mission if we find unsupported item
 	for (size_t i = 0; i < nMissionItems; i++) {
 		struct mission_item_s missionitem;
@@ -190,7 +224,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 			return false;
 		}
 
-		// check if we find unsupported item and reject mission if so
+		// check if we find unsupported items and reject mission if so
 		if (missionitem.nav_cmd != NAV_CMD_IDLE &&
 			missionitem.nav_cmd != NAV_CMD_WAYPOINT &&
 			missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED &&
@@ -207,6 +241,17 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 			mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1));
 			return false;
 		}
+
+		// check if the mission starts with a land command while the vehicle is landed
+		if (missionitem.nav_cmd == NAV_CMD_LAND &&
+			i == 0 &&
+			condition_landed) {
+
+			mavlink_log_critical(_mavlink_fd, "Rejecting mission that starts with LAND command while vehicle is landed.");
+			return false;
+		}
+
+
 	}
 	return true;
 }

src/modules/navigator/mission_feasibility_checker.h

@@ -63,7 +63,7 @@ private:
 	/* Checks for all airframes */
 	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
 	bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false);
-	bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems);
+	bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed);
 	bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued);
 
 	/* Checks specific to fixedwing airframes */
@@ -72,7 +72,7 @@ private:
 	void updateNavigationCapabilities();
 
 	/* Checks specific to rotarywing airframes */
-	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
+	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad);
 public:
 
 	MissionFeasibilityChecker();
@@ -83,7 +83,8 @@ public:
 	 */
 	bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current,
 		size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid,
-		double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued);
+		double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad,
+		bool condition_landed);
 
 };
 

src/modules/navigator/navigator_main.cpp

@@ -616,7 +616,11 @@ Navigator::get_acceptance_radius(float mission_item_radius)
 {
 	float radius = mission_item_radius;
 
-	if (hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) {
+	// XXX only use navigation capabilities for now
+	// when in fixed wing mode
+	// this might need locking against a commanded transition
+	// so that a stale _vstatus doesn't trigger an accepted mission item.
+	if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode && hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) {
 		if (_nav_caps.turn_distance > radius) {
 			radius = _nav_caps.turn_distance;
 		}

src/modules/px4iofirmware/mixer.cpp

@@ -418,7 +418,7 @@ mixer_callback(uintptr_t handle,
  * not loaded faithfully.
  */
 
-static char mixer_text[256];		/* large enough for one mixer */
+static char mixer_text[200];		/* large enough for one mixer */
 static unsigned mixer_text_length = 0;
 
 int

src/modules/px4iofirmware/px4io.c

@@ -82,7 +82,7 @@ static volatile uint8_t msg_next_out, msg_next_in;
  * for mixer handling. Do not allocate more than 80 bytes for
  * output.
  */
-#define NUM_MSG 2
+#define NUM_MSG 1
 static char msg[NUM_MSG][40];
 
 static void heartbeat_blink(void);

src/modules/sdlog2/sdlog2.c

@@ -109,6 +109,7 @@
 #include <uORB/topics/vtol_vehicle_status.h>
 #include <uORB/topics/time_offset.h>
 #include <uORB/topics/mc_att_ctrl_status.h>
+#include <uORB/topics/ekf2_innovations.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -1095,6 +1096,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct time_offset_s time_offset;
 		struct mc_att_ctrl_status_s mc_att_ctrl_status;
 		struct control_state_s ctrl_state;
+		struct ekf2_innovations_s innovations;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -1116,7 +1118,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_STAT_s log_STAT;
 			struct log_VTOL_s log_VTOL;
 			struct log_RC_s log_RC;
-			struct log_OUT0_s log_OUT0;
+			struct log_OUT_s log_OUT;
 			struct log_AIRS_s log_AIRS;
 			struct log_ARSP_s log_ARSP;
 			struct log_FLOW_s log_FLOW;
@@ -1144,6 +1146,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_TSYN_s log_TSYN;
 			struct log_MACS_s log_MACS;
 			struct log_CTS_s log_CTS;
+			struct log_EST4_s log_INO1;
+			struct log_EST5_s log_INO2;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -1160,6 +1164,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int att_sp_sub;
 		int rates_sp_sub;
 		int act_outputs_sub;
+		int act_outputs_1_sub;
 		int act_controls_sub;
 		int act_controls_1_sub;
 		int local_pos_sub;
@@ -1187,6 +1192,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int tsync_sub;
 		int mc_att_ctrl_status_sub;
 		int ctrl_state_sub;
+		int innov_sub;
 	} subs;
 
 	subs.cmd_sub = -1;
@@ -1198,6 +1204,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.att_sp_sub = -1;
 	subs.rates_sp_sub = -1;
 	subs.act_outputs_sub = -1;
+	subs.act_outputs_1_sub = -1;
 	subs.act_controls_sub = -1;
 	subs.act_controls_1_sub = -1;
 	subs.local_pos_sub = -1;
@@ -1222,6 +1229,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.mc_att_ctrl_status_sub = -1;
 	subs.ctrl_state_sub = -1;
 	subs.encoders_sub = -1;
+	subs.innov_sub = -1;
 
 	/* add new topics HERE */
 
@@ -1526,8 +1534,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- ACTUATOR OUTPUTS --- */
 		if (copy_if_updated(ORB_ID(actuator_outputs), &subs.act_outputs_sub, &buf.act_outputs)) {
 			log_msg.msg_type = LOG_OUT0_MSG;
-			memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
+			memcpy(log_msg.body.log_OUT.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT.output));
-			LOGBUFFER_WRITE_AND_COUNT(OUT0);
+			LOGBUFFER_WRITE_AND_COUNT(OUT);
+		}
+
+		if (copy_if_updated(ORB_ID(actuator_outputs), &subs.act_outputs_1_sub, &buf.act_outputs)) {
+			log_msg.msg_type = LOG_OUT1_MSG;
+			memcpy(log_msg.body.log_OUT.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT.output));
+			LOGBUFFER_WRITE_AND_COUNT(OUT);
 		}
 
 		/* --- ACTUATOR CONTROL --- */
@@ -1810,6 +1824,29 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(EST3);
 		}
 
+		/* --- EKF2 INNOVATIONS --- */
+		if (copy_if_updated(ORB_ID(ekf2_innovations), &subs.innov_sub, &buf.innovations)) {
+			log_msg.msg_type = LOG_EST4_MSG;
+			memset(&(log_msg.body.log_INO1.s), 0, sizeof(log_msg.body.log_INO1.s));
+			for (unsigned i = 0; i < 6; i++) {
+				log_msg.body.log_INO1.s[i] = buf.innovations.vel_pos_innov[i];
+				log_msg.body.log_INO1.s[i + 6] = buf.innovations.vel_pos_innov_var[i];
+			}
+			LOGBUFFER_WRITE_AND_COUNT(EST4);
+
+			log_msg.msg_type = LOG_EST5_MSG;
+			memset(&(log_msg.body.log_INO2.s), 0, sizeof(log_msg.body.log_INO2.s));
+			for (unsigned i = 0; i < 3; i++) {
+				log_msg.body.log_INO2.s[i] = buf.innovations.mag_innov[i];
+				log_msg.body.log_INO2.s[i + 3] = buf.innovations.mag_innov_var[i];
+			}
+
+			log_msg.body.log_INO2.s[6] = buf.innovations.heading_innov;
+			log_msg.body.log_INO2.s[7] = buf.innovations.heading_innov_var;
+			LOGBUFFER_WRITE_AND_COUNT(EST5);
+
+		}
+
 		/* --- TECS STATUS --- */
 		if (copy_if_updated(ORB_ID(tecs_status), &subs.tecs_status_sub, &buf.tecs_status)) {
 			log_msg.msg_type = LOG_TECS_MSG;

src/modules/sdlog2/sdlog2_messages.h

@@ -195,9 +195,9 @@ struct log_RC_s {
 	uint32_t frame_drop;
 };
 
-/* --- OUT0 - ACTUATOR_0 OUTPUT --- */
+/* --- OUT - ACTUATOR OUTPUT --- */
 #define LOG_OUT0_MSG 12
-struct log_OUT0_s {
+struct log_OUT_s {
 	float output[8];
 };
 
@@ -416,6 +416,18 @@ struct log_EST3_s {
     float cov[16];
 };
 
+/* --- EST4 - ESTIMATOR INNOVATIONS --- */
+#define LOG_EST4_MSG 48
+struct log_EST4_s {
+    float s[12];
+};
+
+/* --- EST4 - ESTIMATOR INNOVATIONS --- */
+#define LOG_EST5_MSG 49
+struct log_EST5_s {
+    float s[8];
+};
+
 /* --- TEL0..3 - TELEMETRY STATUS --- */
 #define LOG_TEL0_MSG 36
 #define LOG_TEL1_MSG 37
@@ -479,6 +491,8 @@ struct log_MACS_s {
 	float yaw_rate_integ;
 };
 
+/* WARNING: ID 46 is already in use for ATTC1 */
+
 /* --- CONTROL STATE --- */
 #define LOG_CTS_MSG 47
 struct log_CTS_s {
@@ -491,7 +505,7 @@ struct log_CTS_s {
 	float yaw_rate;
 };
 
-/* WARNING: ID 46 is already in use for ATTC1 */
+#define LOG_OUT1_MSG 50
 
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
@@ -535,7 +549,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(VTOL, "f",		"Arsp"),
 	LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"),
 	LOG_FORMAT(RC, "ffffffffffffBBBL",		"C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"),
-	LOG_FORMAT(OUT0, "ffffffff",		"Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
+	LOG_FORMAT_S(OUT0, OUT, "ffffffff",		"Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
+	LOG_FORMAT_S(OUT1, OUT, "ffffffff",		"Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "fff",			"IndSpeed,TrueSpeed,AirTemp"),
 	LOG_FORMAT(ARSP, "fff",			"RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "BffffffLLHhB",	"ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"),
@@ -553,6 +568,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(EST1, "ffffffffffffffff",	"s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
 	LOG_FORMAT(EST2, "ffffffffffff",    "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11"),
 	LOG_FORMAT(EST3, "ffffffffffffffff",    "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"),
+	LOG_FORMAT(EST4, "ffffffffffff", "VxI,VyI,VzI,PxI,PyI,PzI,VxIV,VyIV,VzIV,PxIV,PyIV,PzIV"),
+	LOG_FORMAT(EST5, "ffffffff", "MAGxI,MAGyI,MAGzI,MAGxIV,MAGyIV,MAGzIV,HeadI,HeadIV"),
 	LOG_FORMAT(PWR, "fffBBBBB",		"Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
 	LOG_FORMAT(MOCP, "fffffff",		"QuatW,QuatX,QuatY,QuatZ,X,Y,Z"),
 	LOG_FORMAT(VISN, "ffffffffff",		"X,Y,Z,VX,VY,VZ,QuatW,QuatX,QuatY,QuatZ"),

src/modules/sensors/sensor_params.c

@@ -1842,7 +1842,7 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, -1.0f);
  *
  * @group Battery Calibration
  */
-PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124);	/* scaling for 3DR power brick */
+PARAM_DEFINE_FLOAT(BAT_C_SCALING, -1.0);
 
 
 /**
@@ -2009,6 +2009,15 @@ PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
  */
 PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
+/**
+ * Kill switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Switches
+ */
+PARAM_DEFINE_INT32(RC_MAP_KILL_SW, 0);
+
 /**
  * Flaps channel mapping.
  *
@@ -2243,6 +2252,25 @@ PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f);
  */
 PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f);
 
+
+/**
+ * Threshold for the kill switch
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ * @min -1
+ * @max 1
+ * @group Radio Switches
+ *
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_KILLSWITCH_TH, 0.25f);
+
 /**
  * PWM input channel that provides RSSI.
  *

src/modules/sensors/sensors.cpp

@@ -277,6 +277,7 @@ private:
 		int rc_map_loiter_sw;
 		int rc_map_acro_sw;
 		int rc_map_offboard_sw;
+		int rc_map_kill_sw;
 
 		int rc_map_flaps;
 
@@ -297,6 +298,7 @@ private:
 		float rc_loiter_th;
 		float rc_acro_th;
 		float rc_offboard_th;
+		float rc_killswitch_th;
 		bool rc_assist_inv;
 		bool rc_auto_inv;
 		bool rc_rattitude_inv;
@@ -305,6 +307,7 @@ private:
 		bool rc_loiter_inv;
 		bool rc_acro_inv;
 		bool rc_offboard_inv;
+		bool rc_killswitch_inv;
 
 		float battery_voltage_scaling;
 		float battery_current_scaling;
@@ -336,6 +339,7 @@ private:
 		param_t rc_map_loiter_sw;
 		param_t rc_map_acro_sw;
 		param_t rc_map_offboard_sw;
+		param_t rc_map_kill_sw;
 
 		param_t rc_map_flaps;
 
@@ -360,6 +364,7 @@ private:
 		param_t rc_loiter_th;
 		param_t rc_acro_th;
 		param_t rc_offboard_th;
+		param_t rc_killswitch_th;
 
 		param_t battery_voltage_scaling;
 		param_t battery_current_scaling;
@@ -589,6 +594,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
 	_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
 	_parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW");
+	_parameter_handles.rc_map_kill_sw = param_find("RC_MAP_KILL_SW");
 
 	_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
 	_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -614,6 +620,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
 	_parameter_handles.rc_acro_th = param_find("RC_ACRO_TH");
 	_parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
+	_parameter_handles.rc_killswitch_th = param_find("RC_KILLSWITCH_TH");
 
 	/* Differential pressure offset */
 	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
@@ -655,6 +662,7 @@ Sensors::Sensors() :
 	(void)param_find("PWM_AUX_DISARMED");
 	(void)param_find("TRIG_MODE");
 	(void)param_find("UAVCAN_ENABLE");
+	(void)param_find("LPE_ENABLED");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -774,6 +782,10 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 	}
 
+	if (param_get(_parameter_handles.rc_map_kill_sw, &(_parameters.rc_map_kill_sw)) != OK) {
+		warnx("%s", paramerr);
+	}
+
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
 		warnx("%s", paramerr);
 	}
@@ -813,6 +825,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_offboard_th, &(_parameters.rc_offboard_th));
 	_parameters.rc_offboard_inv = (_parameters.rc_offboard_th < 0);
 	_parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
+	param_get(_parameter_handles.rc_killswitch_th, &(_parameters.rc_killswitch_th));
+	_parameters.rc_killswitch_inv = (_parameters.rc_killswitch_th < 0);
+	_parameters.rc_killswitch_th = fabs(_parameters.rc_killswitch_th);
 
 	/* update RC function mappings */
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -827,6 +842,7 @@ Sensors::parameters_update()
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH] = _parameters.rc_map_kill_sw - 1;
 
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
 
@@ -850,12 +866,13 @@ Sensors::parameters_update()
 
 	} else if (_parameters.battery_voltage_scaling < 0.0f) {
 		/* apply scaling according to defaults if set to default */
-
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
-#if defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
+		_parameters.battery_voltage_scaling = 0.011f;
+#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2)
 		_parameters.battery_voltage_scaling = 0.0082f;
-#elif CONFIG_ARCH_BOARD_AEROCORE
+#elif defined (CONFIG_ARCH_BOARD_AEROCORE)
 		_parameters.battery_voltage_scaling = 0.0063f;
-#elif CONFIG_ARCH_BOARD_PX4FMU_V1
+#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1)
 		_parameters.battery_voltage_scaling = 0.00459340659f;
 #else
 		/* ensure a missing default trips a low voltage lockdown */
@@ -866,6 +883,23 @@ Sensors::parameters_update()
 	/* scaling of ADC ticks to battery current */
 	if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) {
 		warnx("%s", paramerr);
+
+	} else if (_parameters.battery_current_scaling < 0.0f) {
+		/* apply scaling according to defaults if set to default */
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
+		/* current scaling for ACSP4 */
+		_parameters.battery_current_scaling = 0.029296875f;
+#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2)
+		/* current scaling for 3DR power brick */
+		_parameters.battery_current_scaling = 0.0124f;
+#elif defined (CONFIG_ARCH_BOARD_AEROCORE)
+		_parameters.battery_current_scaling = 0.0124f;
+#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1)
+		_parameters.battery_current_scaling = 0.0124f;
+#else
+		/* ensure a missing default leads to an unrealistic current value */
+		_parameters.battery_voltage_scaling = 0.00001f;
+#endif
 	}
 
 	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
@@ -1676,6 +1710,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 
 					_battery_current_timestamp = t;
 
+#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
+
 				} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
 
 					/* calculate airspeed, raw is the difference from */
@@ -1704,6 +1740,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 							_diff_pres_pub = orb_advertise(ORB_ID(differential_pressure), &_diff_pres);
 						}
 					}
+
+#endif
 				}
 			}
 
@@ -1954,6 +1992,8 @@ Sensors::rc_poll()
 					     _parameters.rc_acro_inv);
 			manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD,
 						 _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
+			manual.kill_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH,
+					     _parameters.rc_killswitch_th, _parameters.rc_killswitch_inv);
 
 			/* publish manual_control_setpoint topic */
 			if (_manual_control_pub != nullptr) {

src/modules/systemlib/param/param.c

@@ -44,6 +44,8 @@
 //#include <debug.h>
 #include <px4_defines.h>
 #include <px4_posix.h>
+#include <px4_config.h>
+#include <px4_spi.h>
 #include <string.h>
 #include <stdbool.h>
 #include <stdlib.h>
@@ -235,9 +237,9 @@ param_find_changed(param_t param)
 }
 
 static void
-param_notify_changes(void)
+param_notify_changes(bool is_saved)
 {
-	struct parameter_update_s pup = { .timestamp = hrt_absolute_time() };
+	struct parameter_update_s pup = { .timestamp = hrt_absolute_time(), .saved = is_saved};
 
 	/*
 	 * If we don't have a handle to our topic, create one now; otherwise
@@ -497,7 +499,7 @@ param_get(param_t param, void *val)
 }
 
 static int
-param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes)
+param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved)
 {
 	int result = -1;
 	bool params_changed = false;
@@ -575,7 +577,7 @@ out:
 	 * a thing has been set.
 	 */
 	if (params_changed && notify_changes) {
-		param_notify_changes();
+		param_notify_changes(is_saved);
 	}
 
 	return result;
@@ -584,13 +586,19 @@ out:
 int
 param_set(param_t param, const void *val)
 {
-	return param_set_internal(param, val, false, true);
+	return param_set_internal(param, val, false, true, false);
+}
+
+int
+param_set_no_autosave(param_t param, const void *val)
+{
+	return param_set_internal(param, val, false, true, true);
 }
 
 int
 param_set_no_notification(param_t param, const void *val)
 {
-	return param_set_internal(param, val, false, false);
+	return param_set_internal(param, val, false, false, false);
 }
 
 bool
@@ -643,7 +651,7 @@ param_reset(param_t param)
 	param_unlock();
 
 	if (s != NULL) {
-		param_notify_changes();
+		param_notify_changes(false);
 	}
 
 	return (!param_found);
@@ -663,7 +671,7 @@ param_reset_all(void)
 
 	param_unlock();
 
-	param_notify_changes();
+	param_notify_changes(false);
 }
 
 void
@@ -695,7 +703,7 @@ param_reset_excludes(const char *excludes[], int num_excludes)
 
 	param_unlock();
 
-	param_notify_changes();
+	param_notify_changes(false);
 }
 
 static const char *param_default_file = PX4_ROOTFSDIR"/eeprom/parameters";
@@ -738,7 +746,13 @@ param_save_default(void)
 		return ERROR;
 	}
 
-	res = param_export(fd, false);
+	res = 1;
+	int attempts = 5;
+
+	while (res != OK && attempts > 0) {
+		res = param_export(fd, false);
+		attempts--;
+	}
 
 	if (res != OK) {
 		warnx("failed to write parameters to file: %s", filename);
@@ -779,6 +793,38 @@ param_load_default(void)
 	return 0;
 }
 
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
+//struct spi_dev_s *dev = nullptr;
+irqstate_t state;
+#endif
+
+static void
+param_bus_lock(bool lock)
+{
+
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
+	// FMUv4 has baro and FRAM on the same bus,
+	// as this offers on average a 100% silent
+	// bus for the baro operation
+
+	// XXX this would be the preferred locking method
+	// if (dev == nullptr) {
+	// 	dev = up_spiinitialize(PX4_SPI_BUS_BARO);
+	// }
+
+	// SPI_LOCK(dev, lock);
+
+	// we lock like this for Pixracer for now
+	if (lock) {
+		state = irqsave();
+
+	} else {
+		irqrestore(state);
+	}
+
+#endif
+}
+
 int
 param_export(int fd, bool only_unsaved)
 {
@@ -788,7 +834,9 @@ param_export(int fd, bool only_unsaved)
 
 	param_lock();
 
+	param_bus_lock(true);
 	bson_encoder_init_file(&encoder, fd);
+	param_bus_lock(false);
 
 	/* no modified parameters -> we are done */
 	if (param_values == NULL) {
@@ -813,6 +861,9 @@ param_export(int fd, bool only_unsaved)
 
 		/* append the appropriate BSON type object */
 
+		/* lock as short as possible */
+		param_bus_lock(true);
+
 		switch (param_type(s->param)) {
 
 		case PARAM_TYPE_INT32:
@@ -820,6 +871,7 @@ param_export(int fd, bool only_unsaved)
 
 			if (bson_encoder_append_int(&encoder, param_name(s->param), i)) {
 				debug("BSON append failed for '%s'", param_name(s->param));
+				param_bus_lock(false);
 				goto out;
 			}
 
@@ -830,6 +882,7 @@ param_export(int fd, bool only_unsaved)
 
 			if (bson_encoder_append_double(&encoder, param_name(s->param), f)) {
 				debug("BSON append failed for '%s'", param_name(s->param));
+				param_bus_lock(false);
 				goto out;
 			}
 
@@ -842,6 +895,7 @@ param_export(int fd, bool only_unsaved)
 						       param_size(s->param),
 						       param_get_value_ptr(s->param))) {
 				debug("BSON append failed for '%s'", param_name(s->param));
+				param_bus_lock(false);
 				goto out;
 			}
 
@@ -849,8 +903,14 @@ param_export(int fd, bool only_unsaved)
 
 		default:
 			debug("unrecognized parameter type");
+			param_bus_lock(false);
 			goto out;
 		}
+
+		param_bus_lock(false);
+
+		/* allow this process to be interrupted by another process / thread */
+		usleep(5);
 	}
 
 	result = 0;
@@ -955,7 +1015,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
 		goto out;
 	}
 
-	if (param_set_internal(param, v, state->mark_saved, true)) {
+	if (param_set_internal(param, v, state->mark_saved, true, false)) {
 		debug("error setting value for '%s'", node->name);
 		goto out;
 	}
@@ -984,15 +1044,23 @@ param_import_internal(int fd, bool mark_saved)
 	int result = -1;
 	struct param_import_state state;
 
+	param_bus_lock(true);
+
 	if (bson_decoder_init_file(&decoder, fd, param_import_callback, &state)) {
 		debug("decoder init failed");
+		param_bus_lock(false);
 		goto out;
 	}
 
+	param_bus_lock(false);
+
 	state.mark_saved = mark_saved;
 
 	do {
+		param_bus_lock(true);
 		result = bson_decoder_next(&decoder);
+		usleep(1);
+		param_bus_lock(false);
 
 	} while (result > 0);
 

src/modules/systemlib/param/param.h

@@ -211,6 +211,16 @@ __EXPORT int		param_get(param_t param, void *val);
  */
 __EXPORT int		param_set(param_t param, const void *val);
 
+/**
+ * Set the value of a parameter, but do not trigger an auto-save
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @param val		The value to set; assumed to point to a variable of the parameter type.
+ *			For structures, the pointer is assumed to point to a structure to be copied.
+ * @return		Zero if the parameter's value could be set from a scalar, nonzero otherwise.
+ */
+__EXPORT int		param_set_no_autosave(param_t param, const void *val);
+
 /**
  * Set the value of a parameter, but do not notify the system about the change.
  *

src/modules/uORB/objects_common.cpp

@@ -273,3 +273,6 @@ ORB_DEFINE(camera_trigger, struct camera_trigger_s);
 
 #include "topics/vehicle_command_ack.h"
 ORB_DEFINE(vehicle_command_ack, struct vehicle_command_ack_s);
+
+#include "topics/ekf2_innovations.h"
+ORB_DEFINE(ekf2_innovations, struct ekf2_innovations_s);

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -203,3 +203,12 @@ PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f);
+
+/**
+ * Enable optimal recovery strategy for pitch-weak tailsitters
+ *
+ * @min 0
+ * @max 1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_OPT_RECOV_EN, 0);

src/platforms/posix/main.cpp

@@ -112,10 +112,16 @@ static void run_cmd(const vector<string> &appargs, bool exit_on_fail)
 
 		int retval = apps[command](i, (char **)arg);
 
-		if (exit_on_fail && retval) {
+		if (retval) {
-			exit(retval);
+			cout << "Command failed, returned " << retval << endl;
+
+			if (exit_on_fail && retval) {
+				exit(retval);
+			}
 		}
 
+
+
 	} else if (command.compare("help") == 0) {
 		list_builtins();
 
@@ -126,17 +132,13 @@ static void run_cmd(const vector<string> &appargs, bool exit_on_fail)
 		cout << endl << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl;
 
 	}
-
-	if (exit_on_fail) {
-		print_prompt();
-	}
 }
 
 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." <<
+	cout << "   -d            - Optional flag to run the app in daemon mode and does not 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;
@@ -215,6 +217,7 @@ int main(int argc, char **argv)
 
 			if (infile.is_open()) {
 				for (string line; getline(infile, line, '\n');) {
+					// TODO: this should be true but for that we have to check all startup files
 					process_line(line, false);
 				}
 
@@ -308,9 +311,11 @@ int main(int argc, char **argv)
 						}
 					}
 
-					process_line(mystr, !daemon_mode);
+					process_line(mystr, false);
 					mystr = "";
 					buf_ptr_read = buf_ptr_write;
+
+					print_prompt();
 					break;
 
 				case '\033': {	// arrow keys

src/systemcmds/param/CMakeLists.txt

@@ -33,7 +33,7 @@
 px4_add_module(
 	MODULE systemcmds__param
 	MAIN param
-	STACK 1800
+	STACK 2500
 	COMPILE_FLAGS
 		-Os
 	SRCS

src/systemcmds/param/module.mk

@@ -1,45 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 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.
-#
-############################################################################
-
-#
-# Build the parameters tool.
-#
-
-MODULE_COMMAND	 = param
-SRCS		 = param.c
-
-# Note: measurements yielded a max of 900 bytes used.
-MODULE_STACKSIZE = 1800
-
-MAXOPTIMIZATION	 = -Os
-

src/systemcmds/param/param.c

@@ -448,7 +448,7 @@ do_set(const char *name, const char *val, bool fail_on_not_found)
 				       param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
 				       param_name(param));
 				printf("curr: %ld", (long)i);
-				param_set(param, &newval);
+				param_set_no_autosave(param, &newval);
 				printf(" -> new: %ld\n", (long)newval);
 			}
 		}
@@ -470,7 +470,7 @@ do_set(const char *name, const char *val, bool fail_on_not_found)
 				       param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
 				       param_name(param));
 				printf("curr: %4.4f", (double)f);
-				param_set(param, &newval);
+				param_set_no_autosave(param, &newval);
 				printf(" -> new: %4.4f\n", (double)newval);
 			}
 
@@ -483,7 +483,13 @@ do_set(const char *name, const char *val, bool fail_on_not_found)
 		return 1;
 	}
 
-	return 0;
+	if (param_save_default()) {
+		warnx("Param export failed.");
+		return 1;
+
+	} else {
+		return 0;
+	}
 }
 
 static int

src/systemcmds/tests/CMakeLists.txt

@@ -69,7 +69,7 @@ endif()
 px4_add_module(
 	MODULE systemcmds__tests
 	MAIN tests
-	STACK 60000
+	STACK 8000
 	COMPILE_FLAGS
 		-Wframe-larger-than=6000
 		-Wno-float-equal