updated to master (solve merge conflicts)

.travis.yml

@@ -75,6 +75,8 @@ before_script:
   - mkdir -p ~/bin
   - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++
   - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc
+  - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-size
+  - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-objcopy
   - ln -s /usr/bin/ccache ~/bin/clang++
   - ln -s /usr/bin/ccache ~/bin/clang++-3.4
   - ln -s /usr/bin/ccache ~/bin/clang++-3.5
@@ -98,7 +100,15 @@ script:
   - make check_format
   - arm-none-eabi-gcc --version
   - echo 'Building POSIX Firmware..' && make posix_sitl_simple
-  - echo 'Running Tests..' && make posix_sitl_simple test && cat build_posix_sitl_simple/src/modules/systemlib/mixer/mixer_multirotor.generated.h
+  - echo 'Running Tests..' && make posix_sitl_simple test
+  - echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol
+  - cd vectorcontrol
+  - BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make
+  - cd ..
+  - mkdir -p ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/
+  - mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/
+  - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.*.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/.
+  - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.*.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/.
   - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default
   - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default
   - echo 'Running Tests..' && make px4fmu-v2_default test
@@ -108,7 +118,7 @@ after_success:
          cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4
       && cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v2-default.px4 px4fmu-v2_default.px4
       && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4
-      && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
+      && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/
       && ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/
       && ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/
       && ./CI-Tools/s3cmd-put CI-Tools/index.html index.html

Makefile

@@ -44,10 +44,18 @@ ifneq ($(CMAKE_VER),0)
     $(warning Not a valid CMake version or CMake not installed.)
     $(warning On Ubuntu, install or upgrade via:)
     $(warning )
+    $(warning 3rd party PPA:)
     $(warning sudo add-apt-repository ppa:george-edison55/cmake-3.x -y)
     $(warning sudo apt-get update)
     $(warning sudo apt-get install cmake)
     $(warning )
+    $(warning Official website:)
+    $(warning wget https://cmake.org/files/v3.3/cmake-3.3.2-Linux-x86_64.sh)
+    $(warning chmod +x cmake-3.3.2-Linux-x86_64.sh)
+    $(warning sudo mkdir /opt/cmake-3.3.2)
+    $(warning sudo ./cmake-3.3.2-Linux-x86_64.sh --prefix=/opt/cmake-3.3.2 --exclude-subdir)
+    $(warning export PATH=/opt/cmake-3.3.2/bin:$$PATH)
+    $(warning )
     $(error Fatal)
 endif
 
@@ -156,8 +164,8 @@ posix_sitl_default: posix_sitl_simple
 ros: ros_sitl_simple
 
 sitl_deprecation:
-	@echo "Deprecated. Use 'make posix_sitl_default run_sitl' instead."
-	@echo "Change init script with 'make posix_sitl_default config'"
+	@echo "Deprecated. Use 'make posix_sitl_default jmavsim' or"
+	@echo "'make posix_sitl_default gazebo' if Gazebo is preferred."
 
 sitl_quad: sitl_deprecation
 sitl_plane: sitl_deprecation
@@ -176,7 +184,7 @@ clean:
 # targets handled by cmake
 cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \
 	run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim \
-	jmavsim_gdb jmavsim_lldb
+	jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_vtol gazebo_iris gazebo_vtol
 $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ))))
 
 .PHONY: clean

ROMFS/px4fmu_common/init.d/11001_hexa_cox

@@ -4,6 +4,17 @@
 #
 # @type Hexarotor Coaxial
 #
+# @output MAIN1 front right top, CW; angle:60; direction:CW
+# @output MAIN2 front right bottom, CCW; angle:60; direction:CCW
+# @output MAIN3 back top, CW; angle:180; direction:CW
+# @output MAIN4 back bottom, CCW; angle:180; direction:CCW
+# @output MAIN5 front left top, CW; angle:-60; direction:CW
+# @output MAIN6 front left bottom, CCW;angle:-60; direction:CCW 
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
 # @maintainer Lorenz Meier <lorenz@px4.io>
 #
 

ROMFS/px4fmu_common/init.d/3033_wingwing

@@ -14,7 +14,7 @@
 # @output AUX2 feed-through of RC AUX2 channel
 # @output AUX3 feed-through of RC AUX3 channel
 #
-# @maintainer Simon Wilks <simon@px4.io>
+# @maintainer Lorenz Meier <lorenz@px4.io>
 #
 
 sh /etc/init.d/rc.fw_defaults
@@ -39,6 +39,9 @@ then
 	param set FW_RR_P 0.04
 fi
 
+# Configure this as plane
+set MAV_TYPE 1
+# Set mixer
 set MIXER wingwing
 # Provide ESC a constant 1000 us pulse
 set PWM_OUT 4

ROMFS/px4fmu_common/init.d/4009_qav250

@@ -0,0 +1,34 @@
+#!nsh
+#
+# @name Lumenier QAV250
+#
+# @type Quadrotor x
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Mark Whitehorn <kd0aij@gmail.com>
+#
+
+sh /etc/init.d/4001_quad_x
+
+if [ $AUTOCNF == yes ]
+then
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.08
+	param set MC_ROLLRATE_I 0.05
+	param set MC_ROLLRATE_D 0.003
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.08
+	param set MC_PITCHRATE_I 0.1
+	param set MC_PITCHRATE_D 0.003
+	param set MC_YAW_P 2.8
+	param set MC_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.1
+	param set MC_YAWRATE_D 0.0
+	param set PWM_MIN 1075
+	param set PWM_THR_MIN 0.06
+	param set PWM_MANTHR_MIN 0.06
+fi
+

ROMFS/px4fmu_common/init.d/rcS

@@ -391,6 +391,14 @@ then
 	#
 	set TTYS1_BUSY no
 
+	#
+	# Check if UAVCAN is enabled, default to it for ESCs
+	#
+	if param greater UAVCAN_ENABLE 2
+	then
+		set OUTPUT_MODE uavcan_esc
+	fi
+
 	# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
 	if [ $OUTPUT_MODE != none ]
 	then

Tools/px4airframes/xmlout.py

@@ -72,9 +72,13 @@ class XMLOutput():
                             xml_field.text = value
                     for code in param.GetOutputCodes():
                         value = param.GetOutputValue(code)
+                        valstrs = value.split(";")
                         xml_field = ET.SubElement(xml_param, "output")
                         xml_field.attrib["name"] = code
-                        xml_field.text = value
+                        for attrib in valstrs[1:]:
+                            attribstrs = attrib.split(":")
+                            xml_field.attrib[attribstrs[0].strip()] = attribstrs[1].strip()
+                        xml_field.text = valstrs[0]
                 if last_param_name != param.GetName():
                     board_specific_param_set = False
         indent(xml_parameters)

Tools/px_romfs_pruner.py

@@ -57,7 +57,7 @@ def main():
         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:
+                        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)
@@ -74,8 +74,7 @@ def main():
                                         else:
                                                 if not line.isspace() and not line.strip().startswith("#"):
                                                         pruned_content += line
-
-                        # overwrite old scratch file
+                       # overwrite old scratch file
                         with open(file_path, "wb") as f:
                                 f.write(pruned_content.encode("ascii", errors='strict'))
 

Tools/sitl_run.sh

@@ -3,18 +3,27 @@
 rc_script=$1
 debugger=$2
 program=$3
-build_path=$4
+model=$4
+build_path=$5
 curr_dir=`pwd`
 
 echo SITL ARGS
 echo rc_script: $rc_script
 echo debugger: $debugger
 echo program: $program
+echo model: $model
 echo build_path: $build_path
 
-if [ "$#" != 4 ]
+if [ "$model" == "" ] || [ "$model" == "none" ]
 then
-	echo usage: sitl_run.sh rc_script debugger program build_path
+	echo "empty model, setting iris as default"
+	model="iris"
+fi
+
+if [ "$#" != 5 ]
+then
+	echo usage: sitl_run.sh rc_script debugger program model build_path
+	echo ""
 	exit 1
 fi
 
@@ -33,13 +42,13 @@ cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit
 
 SIM_PID=0
 
-if [ "$program" == "jmavsim" ]
+if [ "$program" == "jmavsim" ] && [ "$no_sim" == "" ]
 then
 	cd Tools/jMAVSim
 	ant
 	java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 &
 	SIM_PID=`echo $!`
-elif [ "$3" == "gazebo" ]
+elif [ "$3" == "gazebo" ] && [ "$no_sim" == "" ]
 then
 	if [ -x "$(command -v gazebo)" ]
 	then
@@ -54,8 +63,10 @@ then
 		cd Tools/sitl_gazebo/Build
 		cmake ..
 		make -j4
-		gazebo ../worlds/iris.world &
+		gzserver ../worlds/${model}.world &
 		SIM_PID=`echo $!`
+		gzclient &
+		GUI_PID=`echo $!`
 	else
 		echo "You need to have gazebo simulator installed!"
 		exit 1
@@ -76,10 +87,11 @@ else
 	./mainapp ../../../../${rc_script}_${program}
 fi
 
-if [ "$3" == "jmavsim" ]
+if [ "$program" == "jmavsim" ]
 then
 	kill -9 $SIM_PID
-elif [ "$3" == "gazebo" ]
+elif [ "$program" == "gazebo" ]
 then
 	kill -9 $SIM_PID
+	kill -9 $GUI_PID
 fi

cmake/common/px4_base.cmake

@@ -700,7 +700,7 @@ function(px4_create_git_hash_header)
 		REQUIRED HEADER 
 		ARGN ${ARGN})
 	execute_process(
-		COMMAND git log -n 1 --pretty=format:"%H"
+		COMMAND git rev-parse HEAD
 		OUTPUT_VARIABLE git_desc
 		OUTPUT_STRIP_TRAILING_WHITESPACE
 		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}

cmake/configs/nuttx_px4fmu-v2_default.cmake

@@ -37,7 +37,7 @@ set(config_module_list
 	drivers/meas_airspeed
 	drivers/frsky_telemetry
 	modules/sensors
-	drivers/mkblctrl
+	#drivers/mkblctrl
 	drivers/px4flow
 	drivers/oreoled
 	drivers/gimbal
@@ -54,7 +54,7 @@ set(config_module_list
 	systemcmds/pwm
 	systemcmds/esc_calib
 	systemcmds/reboot
-	systemcmds/topic_listener
+	#systemcmds/topic_listener
 	systemcmds/top
 	systemcmds/config
 	systemcmds/nshterm

cmake/templates/build_git_version.h.in

@@ -1,4 +1,4 @@
 /* Auto Magically Generated file */
 /* Do not edit! */
-#define PX4_GIT_VERSION_STR  @git_desc@
+#define PX4_GIT_VERSION_STR  "@git_desc@"
 #define PX4_GIT_VERSION_BINARY 0x@git_desc_short@

msg/home_position.msg

@@ -8,3 +8,8 @@ float32 alt				# Altitude in meters (AMSL)
 float32 x				# X coordinate in meters
 float32 y				# Y coordinate in meters
 float32 z				# Z coordinate in meters
+
+float32 yaw				# Yaw angle in radians
+float32 direction_x		# Takeoff direction in NED X
+float32 direction_y		# Takeoff direction in NED Y
+float32 direction_z		# Takeoff direction in NED Z

msg/manual_control_setpoint.msg

@@ -38,6 +38,7 @@ 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 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

msg/rc_channels.msg

@@ -1,4 +1,4 @@
-int32 RC_CHANNELS_FUNCTION_MAX=19
+int32 RC_CHANNELS_FUNCTION_MAX=20
 uint8 RC_CHANNELS_FUNCTION_THROTTLE=0
 uint8 RC_CHANNELS_FUNCTION_ROLL=1
 uint8 RC_CHANNELS_FUNCTION_PITCH=2
@@ -18,10 +18,11 @@ uint8 RC_CHANNELS_FUNCTION_AUX_5=15
 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
 uint64 timestamp						# Timestamp in microseconds since boot time
 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
-float32[19] channels						# Scaled to -1..1 (throttle: 0..1)
+float32[20] channels						# Scaled to -1..1 (throttle: 0..1)
 uint8 channel_count						# Number of valid channels
-int8[19] function						# Functions mapping
+int8[20] function						# Functions mapping
 uint8 rssi							# Receive signal strength index
 bool signal_lost						# Control signal lost, should be checked together with topic timeout

msg/telemetry_status.msg

@@ -2,6 +2,7 @@ uint8 TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0
 uint8 TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1
 uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2
 uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3
+uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4
 
 uint64 timestamp
 uint64 heartbeat_time			# Time of last received heartbeat from remote system

msg/vehicle_status.msg

@@ -8,7 +8,8 @@ uint8 MAIN_STATE_AUTO_RTL = 5
 uint8 MAIN_STATE_ACRO = 6
 uint8 MAIN_STATE_OFFBOARD = 7
 uint8 MAIN_STATE_STAB = 8
-uint8 MAIN_STATE_MAX = 9
+uint8 MAIN_STATE_RATTITUDE = 9
+uint8 MAIN_STATE_MAX = 10
 
 # If you change the order, add or remove arming_state_t states make sure to update the arrays
 # in state_machine_helper.cpp as well.
@@ -41,7 +42,8 @@ uint8 NAVIGATION_STATE_DESCEND = 12		# Descend mode (no position control)
 uint8 NAVIGATION_STATE_TERMINATION = 13		# Termination mode
 uint8 NAVIGATION_STATE_OFFBOARD = 14
 uint8 NAVIGATION_STATE_STAB = 15		# Stabilized mode
-uint8 NAVIGATION_STATE_MAX = 16
+uint8 NAVIGATION_STATE_RATTITUDE = 16		# Rattitude (aka "flip") mode
+uint8 NAVIGATION_STATE_MAX = 17
 
 # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol
 uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128

src/drivers/device/vdev.cpp

@@ -67,6 +67,7 @@ private:
 
 #define PX4_MAX_DEV 500
 static px4_dev_t *devmap[PX4_MAX_DEV];
+pthread_mutex_t devmutex = PTHREAD_MUTEX_INITIALIZER;
 
 /*
  * The standard NuttX operation dispatch table can't call C++ member functions
@@ -142,8 +143,12 @@ VDev::register_driver(const char *name, void *data)
 
 	// Make sure the device does not already exist
 	// FIXME - convert this to a map for efficiency
+
+	pthread_mutex_lock(&devmutex);
+
 	for (int i = 0; i < PX4_MAX_DEV; ++i) {
 		if (devmap[i] && (strcmp(devmap[i]->name, name) == 0)) {
+			pthread_mutex_unlock(&devmutex);
 			return -EEXIST;
 		}
 	}
@@ -157,6 +162,8 @@ VDev::register_driver(const char *name, void *data)
 		}
 	}
 
+	pthread_mutex_unlock(&devmutex);
+
 	if (ret != PX4_OK) {
 		PX4_ERR("No free devmap entries - increase PX4_MAX_DEV");
 	}
@@ -174,6 +181,8 @@ VDev::unregister_driver(const char *name)
 		return -EINVAL;
 	}
 
+	pthread_mutex_lock(&devmutex);
+
 	for (int i = 0; i < PX4_MAX_DEV; ++i) {
 		if (devmap[i] && (strcmp(name, devmap[i]->name) == 0)) {
 			delete devmap[i];
@@ -184,6 +193,8 @@ VDev::unregister_driver(const char *name)
 		}
 	}
 
+	pthread_mutex_unlock(&devmutex);
+
 	return ret;
 }
 
@@ -194,15 +205,20 @@ VDev::unregister_class_devname(const char *class_devname, unsigned class_instanc
 	char name[32];
 	snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
 
+	pthread_mutex_lock(&devmutex);
+
 	for (int i = 0; i < PX4_MAX_DEV; ++i) {
 		if (devmap[i] && strcmp(devmap[i]->name, name) == 0) {
 			delete devmap[i];
 			PX4_DEBUG("Unregistered class DEV %s", name);
 			devmap[i] = NULL;
+			pthread_mutex_unlock(&devmutex);
 			return PX4_OK;
 		}
 	}
 
+	pthread_mutex_unlock(&devmutex);
+
 	return -EINVAL;
 }
 
@@ -497,15 +513,20 @@ VDev *VDev::getDev(const char *path)
 	PX4_DEBUG("VDev::getDev");
 	int i = 0;
 
+	pthread_mutex_lock(&devmutex);
+
 	for (; i < PX4_MAX_DEV; ++i) {
 		//if (devmap[i]) {
 		//	printf("%s %s\n", devmap[i]->name, path);
 		//}
 		if (devmap[i] && (strcmp(devmap[i]->name, path) == 0)) {
+			pthread_mutex_unlock(&devmutex);
 			return (VDev *)(devmap[i]->cdev);
 		}
 	}
 
+	pthread_mutex_unlock(&devmutex);
+
 	return NULL;
 }
 
@@ -514,11 +535,15 @@ void VDev::showDevices()
 	int i = 0;
 	PX4_INFO("Devices:");
 
+	pthread_mutex_lock(&devmutex);
+
 	for (; i < PX4_MAX_DEV; ++i) {
 		if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) {
 			PX4_INFO("   %s", devmap[i]->name);
 		}
 	}
+
+	pthread_mutex_unlock(&devmutex);
 }
 
 void VDev::showTopics()
@@ -526,11 +551,15 @@ void VDev::showTopics()
 	int i = 0;
 	PX4_INFO("Devices:");
 
+	pthread_mutex_lock(&devmutex);
+
 	for (; i < PX4_MAX_DEV; ++i) {
 		if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) == 0) {
 			PX4_INFO("   %s", devmap[i]->name);
 		}
 	}
+
+	pthread_mutex_unlock(&devmutex);
 }
 
 void VDev::showFiles()
@@ -538,12 +567,16 @@ void VDev::showFiles()
 	int i = 0;
 	PX4_INFO("Files:");
 
+	pthread_mutex_lock(&devmutex);
+
 	for (; i < PX4_MAX_DEV; ++i) {
 		if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) != 0 &&
 		    strncmp(devmap[i]->name, "/dev/", 5) != 0) {
 			PX4_INFO("   %s", devmap[i]->name);
 		}
 	}
+
+	pthread_mutex_unlock(&devmutex);
 }
 
 const char *VDev::topicList(unsigned int *next)

src/drivers/device/vdev_posix.cpp

@@ -52,6 +52,8 @@
 
 using namespace device;
 
+pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER;
+
 extern "C" {
 
 	static void timer_cb(void *data)
@@ -68,7 +70,27 @@ extern "C" {
 
 	inline bool valid_fd(int fd)
 	{
-		return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL);
+		pthread_mutex_lock(&filemutex);
+		bool ret = (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL);
+		pthread_mutex_unlock(&filemutex);
+		return ret;
+	}
+
+	inline VDev *get_vdev(int fd)
+	{
+		pthread_mutex_lock(&filemutex);
+		bool valid = (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL);
+		VDev *dev;
+
+		if (valid) {
+			dev = (VDev *)(filemap[fd]->vdev);
+
+		} else {
+			dev = nullptr;
+		}
+
+		pthread_mutex_unlock(&filemutex);
+		return dev;
 	}
 
 	int px4_open(const char *path, int flags, ...)
@@ -93,6 +115,9 @@ extern "C" {
 		}
 
 		if (dev) {
+
+			pthread_mutex_lock(&filemutex);
+
 			for (i = 0; i < PX4_MAX_FD; ++i) {
 				if (filemap[i] == 0) {
 					filemap[i] = new device::file_t(flags, dev, i);
@@ -100,6 +125,8 @@ extern "C" {
 				}
 			}
 
+			pthread_mutex_unlock(&filemutex);
+
 			if (i < PX4_MAX_FD) {
 				ret = dev->open(filemap[i]);
 
@@ -125,11 +152,14 @@ extern "C" {
 	{
 		int ret;
 
-		if (valid_fd(fd)) {
-			VDev *dev = (VDev *)(filemap[fd]->vdev);
-			PX4_DEBUG("px4_close fd = %d", fd);
+		VDev *dev = get_vdev(fd);
+
+		if (dev) {
+			pthread_mutex_lock(&filemutex);
 			ret = dev->close(filemap[fd]);
-			filemap[fd] = NULL;
+			filemap[fd] = nullptr;
+			pthread_mutex_unlock(&filemutex);
+			PX4_DEBUG("px4_close fd = %d", fd);
 
 		} else {
 			ret = -EINVAL;
@@ -147,8 +177,9 @@ extern "C" {
 	{
 		int ret;
 
-		if (valid_fd(fd)) {
-			VDev *dev = (VDev *)(filemap[fd]->vdev);
+		VDev *dev = get_vdev(fd);
+
+		if (dev) {
 			PX4_DEBUG("px4_read fd = %d", fd);
 			ret = dev->read(filemap[fd], (char *)buffer, buflen);
 
@@ -168,8 +199,9 @@ extern "C" {
 	{
 		int ret;
 
-		if (valid_fd(fd)) {
-			VDev *dev = (VDev *)(filemap[fd]->vdev);
+		VDev *dev = get_vdev(fd);
+
+		if (dev) {
 			PX4_DEBUG("px4_write fd = %d", fd);
 			ret = dev->write(filemap[fd], (const char *)buffer, buflen);
 
@@ -190,8 +222,9 @@ extern "C" {
 		PX4_DEBUG("px4_ioctl fd = %d", fd);
 		int ret = 0;
 
-		if (valid_fd(fd)) {
-			VDev *dev = (VDev *)(filemap[fd]->vdev);
+		VDev *dev = get_vdev(fd);
+
+		if (dev) {
 			ret = dev->ioctl(filemap[fd], cmd, arg);
 
 		} else {
@@ -221,9 +254,10 @@ extern "C" {
 			fds[i].revents = 0;
 			fds[i].priv    = NULL;
 
+			VDev *dev = get_vdev(fds[i].fd);
+
 			// If fd is valid
-			if (valid_fd(fds[i].fd)) {
-				VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
+			if (dev) {
 				PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd);
 				ret = dev->poll(filemap[fds[i].fd], &fds[i], true);
 
@@ -251,9 +285,11 @@ extern "C" {
 
 			// For each fd
 			for (i = 0; i < nfds; ++i) {
+
+				VDev *dev = get_vdev(fds[i].fd);
+
 				// If fd is valid
-				if (valid_fd(fds[i].fd)) {
-					VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);;
+				if (dev) {
 					PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd);
 					ret = dev->poll(filemap[fds[i].fd], &fds[i], false);
 

src/drivers/ll40ls/LidarLiteI2C.cpp

@@ -114,35 +114,33 @@ int LidarLiteI2C::init()
 
 	/* do I2C init (and probe) first */
 	if (I2C::init() != OK) {
-		goto out;
+		return ret;
 	}
 
 	/* allocate basic report buffers */
 	_reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s));
 
 	if (_reports == nullptr) {
-		goto out;
+		return ret;
 	}
 
 	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
 
-	if (_class_instance == CLASS_DEVICE_PRIMARY) {
-		/* get a publish handle on the range finder topic */
-		struct distance_sensor_s ds_report;
-		measure();
-		_reports->get(&ds_report);
-		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
-					 &_orb_class_instance, ORB_PRIO_LOW);
+	/* get a publish handle on the range finder topic */
+	struct distance_sensor_s ds_report = {};
+	measure();
+	_reports->get(&ds_report);
+	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+				 &_orb_class_instance, ORB_PRIO_LOW);
 
-		if (_distance_sensor_topic == nullptr) {
-			DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
-		}
+	if (_distance_sensor_topic == nullptr) {
+		DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
 	}
 
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
 	_sensor_ok = true;
-out:
+
 	return ret;
 }
 

src/drivers/ll40ls/LidarLitePWM.cpp

@@ -108,17 +108,15 @@ int LidarLitePWM::init()
 
 	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
 
-	if (_class_instance == CLASS_DEVICE_PRIMARY) {
-		/* get a publish handle on the distance_sensor topic */
-		struct distance_sensor_s ds_report;
-		measure();
-		_reports->get(&ds_report);
-		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
-					 &_orb_class_instance, ORB_PRIO_LOW);
+	/* get a publish handle on the distance_sensor topic */
+	struct distance_sensor_s ds_report = {};
+	measure();
+	_reports->get(&ds_report);
+	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+				 &_orb_class_instance, ORB_PRIO_LOW);
 
-		if (_distance_sensor_topic == nullptr) {
-			DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
-		}
+	if (_distance_sensor_topic == nullptr) {
+		DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?");
 	}
 
 	return OK;

src/drivers/mb12xx/mb12xx.cpp

@@ -255,7 +255,7 @@ MB12XX::init()
 
 	/* do I2C init (and probe) first */
 	if (I2C::init() != OK) {
-		goto out;
+		return ret;
 	}
 
 	/* allocate basic report buffers */
@@ -265,21 +265,19 @@ MB12XX::init()
 	set_address(_index_counter);		/* set I2c port to temp sonar i2c adress */
 
 	if (_reports == nullptr) {
-		goto out;
+		return ret;
 	}
 
 	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
 
-	if (_class_instance == CLASS_DEVICE_PRIMARY) {
-		/* get a publish handle on the range finder topic */
-		struct distance_sensor_s ds_report = {};
+	/* get a publish handle on the range finder topic */
+	struct distance_sensor_s ds_report = {};
 
-		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
-					 &_orb_class_instance, ORB_PRIO_LOW);
+	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+				 &_orb_class_instance, ORB_PRIO_LOW);
 
-		if (_distance_sensor_topic == nullptr) {
-			DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
-		}
+	if (_distance_sensor_topic == nullptr) {
+		DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
 	}
 
 	// XXX we should find out why we need to wait 200 ms here
@@ -321,7 +319,7 @@ MB12XX::init()
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
 	_sensor_ok = true;
-out:
+
 	return ret;
 }
 

src/drivers/pwm_out_sim/pwm_out_sim.cpp

@@ -79,6 +79,8 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_0.h>
 #include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_outputs.h>
 
@@ -94,6 +96,7 @@ public:
 	enum Mode {
 		MODE_2PWM,
 		MODE_4PWM,
+		MODE_6PWM,
 		MODE_8PWM,
 		MODE_12PWM,
 		MODE_16PWM,
@@ -111,23 +114,29 @@ public:
 	int		_task;
 
 private:
-	static const unsigned _max_actuators = 4;
+	static const unsigned _max_actuators = 8;
 
 	Mode		_mode;
 	int 		_update_rate;
 	int 		_current_update_rate;
-	int		_t_actuators;
-	int		_t_armed;
-	orb_advert_t	_t_outputs;
+	int			_control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+	px4_pollfd_struct_t	_poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+	unsigned	_poll_fds_num;
+	int		_armed_sub;
+	orb_advert_t	_outputs_pub;
 	unsigned	_num_outputs;
 	bool		_primary_pwm_device;
 
+	uint32_t	_groups_required;
+	uint32_t	_groups_subscribed;
+
 	volatile bool	_task_should_exit;
 	static bool	_armed;
 
 	MixerGroup	*_mixers;
 
-	actuator_controls_s _controls;
+	actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
+	orb_id_t	_control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS];
 
 	static void	task_main_trampoline(int argc, char *argv[]);
 	void		task_main();
@@ -138,6 +147,7 @@ private:
 					 float &input);
 
 	int		pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg);
+	void 	subscribe();
 
 	struct GPIOConfig {
 		uint32_t	input;
@@ -176,15 +186,24 @@ PWMSim::PWMSim() :
 	_mode(MODE_NONE),
 	_update_rate(50),
 	_current_update_rate(0),
-	_t_actuators(-1),
-	_t_armed(-1),
-	_t_outputs(0),
+	_control_subs { -1},
+	_poll_fds_num(0),
+	_armed_sub(-1),
+	_outputs_pub(0),
 	_num_outputs(0),
 	_primary_pwm_device(false),
+	_groups_required(0),
+	_groups_subscribed(0),
 	_task_should_exit(false),
 	_mixers(nullptr)
 {
 	_debug_enabled = true;
+	memset(_controls, 0, sizeof(_controls));
+
+	_control_topics[0] = ORB_ID(actuator_controls_0);
+	_control_topics[1] = ORB_ID(actuator_controls_1);
+	_control_topics[2] = ORB_ID(actuator_controls_2);
+	_control_topics[3] = ORB_ID(actuator_controls_3);
 }
 
 PWMSim::~PWMSim()
@@ -326,64 +345,62 @@ PWMSim::set_pwm_rate(unsigned rate)
 	return OK;
 }
 
+void
+PWMSim::subscribe()
+{
+	/* subscribe/unsubscribe to required actuator control groups */
+	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
+	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
+	_poll_fds_num = 0;
+
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+		if (sub_groups & (1 << i)) {
+			PX4_DEBUG("subscribe to actuator_controls_%d", i);
+			_control_subs[i] = orb_subscribe(_control_topics[i]);
+		}
+
+		if (unsub_groups & (1 << i)) {
+			PX4_DEBUG("unsubscribe from actuator_controls_%d", i);
+			px4_close(_control_subs[i]);
+			_control_subs[i] = -1;
+		}
+
+		if (_control_subs[i] > 0) {
+			printf("valid\n");
+			_poll_fds[_poll_fds_num].fd = _control_subs[i];
+			_poll_fds[_poll_fds_num].events = POLLIN;
+			_poll_fds_num++;
+		}
+	}
+}
+
 void
 PWMSim::task_main()
 {
-	/*
-	 * Subscribe to the appropriate PWM output topic based on whether we are the
-	 * primary PWM output or not.
-	 */
-	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
-				     ORB_ID(actuator_controls_1));
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
+	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	orb_set_interval(_armed_sub, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
 
 	/* advertise the mixed control outputs, insist on the first group output */
-	_t_outputs = orb_advertise(ORB_ID(actuator_outputs), &outputs);
+	_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &outputs);
 
-	px4_pollfd_struct_t fds[2];
-	fds[0].fd = _t_actuators;
-	fds[0].events = POLLIN;
-	fds[1].fd = _t_armed;
-	fds[1].events = POLLIN;
-
-	unsigned num_outputs;
-
-	/* select the number of virtual outputs */
-	switch (_mode) {
-	case MODE_2PWM:
-		num_outputs = 2;
-		break;
-
-	case MODE_4PWM:
-		num_outputs = 4;
-		break;
-
-	case MODE_8PWM:
-	case MODE_12PWM:
-	case MODE_16PWM:
-		// XXX only support the lower 8 - trivial to extend
-		num_outputs = 8;
-		break;
-
-	case MODE_NONE:
-	default:
-		num_outputs = 0;
-		break;
-	}
 
 	DEVICE_LOG("starting");
 
 	/* loop until killed */
 	while (!_task_should_exit) {
 
+		if (_groups_subscribed != _groups_required) {
+			subscribe();
+			_groups_subscribed = _groups_required;
+		}
+
 		/* handle update rate changes */
 		if (_current_update_rate != _update_rate) {
 			int update_rate_in_ms = int(1000 / _update_rate);
@@ -392,13 +409,18 @@ PWMSim::task_main()
 				update_rate_in_ms = 2;
 			}
 
-			orb_set_interval(_t_actuators, update_rate_in_ms);
+			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+				if (_control_subs[i] > 0) {
+					orb_set_interval(_control_subs[i], update_rate_in_ms);
+				}
+			}
+
 			// up_pwm_servo_set_rate(_update_rate);
 			_current_update_rate = _update_rate;
 		}
 
 		/* sleep waiting for data, but no more than a second */
-		int ret = px4_poll(&fds[0], 2, 1000);
+		int ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000);
 
 		/* this would be bad... */
 		if (ret < 0) {
@@ -406,56 +428,107 @@ PWMSim::task_main()
 			continue;
 		}
 
-		/* do we have a control update? */
-		if (fds[0].revents & POLLIN) {
-			/* get controls - must always do this to avoid spinning */
-			orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
-				 ORB_ID(actuator_controls_1), _t_actuators, &_controls);
+		if (ret == 0) {
+			// timeout
+			continue;
+		}
 
-			/* can we mix? */
-			if (_armed && _mixers != nullptr) {
-				/* do mixing */
-				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
-				outputs.timestamp = hrt_absolute_time();
+		/* get controls for required topics */
+		unsigned poll_id = 0;
 
-				/* iterate actuators */
-				for (unsigned i = 0; i < num_outputs; i++) {
-					/* last resort: catch NaN, INF and out-of-band errors */
-					if (i < outputs.noutputs &&
-					    PX4_ISFINITE(outputs.output[i]) &&
-					    outputs.output[i] >= -1.0f &&
-					    outputs.output[i] <= 1.0f) {
-						/* scale for PWM output 900 - 2100us */
-						outputs.output[i] = 1500 + (600 * outputs.output[i]);
-
-					} else {
-						/*
-						 * Value is NaN, INF or out of band - set to the minimum value.
-						 * This will be clearly visible on the servo status and will limit the risk of accidentally
-						 * spinning motors. It would be deadly in flight.
-						 */
-						outputs.output[i] = 900;
-					}
+		for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+			if (_control_subs[i] > 0) {
+				if (_poll_fds[poll_id].revents & POLLIN) {
+					orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
 				}
 
-				/* and publish for anyone that cares to see */
-				orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs);
+				poll_id++;
 			}
 		}
 
-		/* how about an arming update? */
-		if (fds[1].revents & POLLIN) {
-			actuator_armed_s aa;
+		/* can we mix? */
+		if (_armed && _mixers != nullptr) {
 
-			/* get new value */
-			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
+			size_t num_outputs;
+
+			switch (_mode) {
+			case MODE_2PWM:
+				num_outputs = 2;
+				break;
+
+			case MODE_4PWM:
+				num_outputs = 4;
+				break;
+
+			case MODE_6PWM:
+				num_outputs = 6;
+				break;
+
+			case MODE_8PWM:
+				num_outputs = 8;
+				break;
+
+			default:
+				num_outputs = 0;
+				break;
+			}
+
+			/* do mixing */
+			actuator_outputs_s outputs;
+			num_outputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
+			outputs.timestamp = hrt_absolute_time();
+
+			/* disable unused ports by setting their output to NaN */
+			for (size_t i = 0; i < sizeof(outputs.output) / sizeof(outputs.output[0]); i++) {
+				if (i >= num_outputs) {
+					outputs.output[i] = NAN;
+				}
+			}
+
+			/* iterate actuators */
+			for (unsigned i = 0; i < num_outputs; i++) {
+				/* last resort: catch NaN, INF and out-of-band errors */
+				if (i < outputs.noutputs &&
+				    PX4_ISFINITE(outputs.output[i]) &&
+				    outputs.output[i] >= -1.0f &&
+				    outputs.output[i] <= 1.0f) {
+					/* scale for PWM output 900 - 2100us */
+					outputs.output[i] = 1500 + (600 * outputs.output[i]);
+
+				} else {
+					/*
+					 * Value is NaN, INF or out of band - set to the minimum value.
+					 * This will be clearly visible on the servo status and will limit the risk of accidentally
+					 * spinning motors. It would be deadly in flight.
+					 */
+					outputs.output[i] = 900;
+				}
+			}
+
+
+			/* and publish for anyone that cares to see */
+			orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
+		}
+
+		/* how about an arming update? */
+		bool updated;
+		actuator_armed_s aa;
+		orb_check(_armed_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(actuator_armed), _armed_sub, &aa);
 			/* do not obey the lockdown value, as lockdown is for PWMSim */
 			_armed = aa.armed;
 		}
 	}
 
-	px4_close(_t_actuators);
-	px4_close(_t_armed);
+	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+		if (_control_subs[i] > 0) {
+			px4_close(_control_subs[i]);
+		}
+	}
+
+	px4_close(_armed_sub);
 
 	/* make sure servos are off */
 	// up_pwm_servo_deinit();
@@ -671,6 +744,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 		if (_mixers != nullptr) {
 			delete _mixers;
 			_mixers = nullptr;
+			_groups_required = 0;
 		}
 
 		break;
@@ -683,6 +757,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 
 			if (mixer->check()) {
 				delete mixer;
+				_groups_required = 0;
 				ret = -EINVAL;
 
 			} else {
@@ -691,6 +766,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 								 (uintptr_t)&_controls);
 
 				_mixers->add_mixer(mixer);
+				_mixers->groups_required(_groups_required);
 			}
 
 			break;
@@ -705,6 +781,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 			}
 
 			if (_mixers == nullptr) {
+				_groups_required = 0;
 				ret = -ENOMEM;
 
 			} else {
@@ -715,7 +792,11 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 					DEVICE_DEBUG("mixer load failed with %d", ret);
 					delete _mixers;
 					_mixers = nullptr;
+					_groups_required = 0;
 					ret = -EINVAL;
+
+				} else {
+					_mixers->groups_required(_groups_required);
 				}
 			}
 
@@ -772,7 +853,7 @@ hil_new_mode(PortMode new_mode)
 
 	case PORT1_FULL_PWM:
 		/* select 4-pin PWM mode */
-		servo_mode = PWMSim::MODE_4PWM;
+		servo_mode = PWMSim::MODE_8PWM;
 		break;
 
 	case PORT1_PWM_AND_SERIAL:

src/drivers/px4flow/px4flow.cpp

@@ -229,34 +229,32 @@ PX4FLOW::init()
 
 	/* do I2C init (and probe) first */
 	if (I2C::init() != OK) {
-		goto out;
+		return ret;
 	}
 
 	/* allocate basic report buffers */
 	_reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s));
 
 	if (_reports == nullptr) {
-		goto out;
+		return ret;
 	}
 
 	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
 
-	if (_class_instance == CLASS_DEVICE_PRIMARY) {
-		/* get a publish handle on the range finder topic */
-		struct distance_sensor_s ds_report = {};
+	/* get a publish handle on the range finder topic */
+	struct distance_sensor_s ds_report = {};
 
-		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
-					 &_orb_class_instance, ORB_PRIO_HIGH);
+	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+				 &_orb_class_instance, ORB_PRIO_HIGH);
 
-		if (_distance_sensor_topic == nullptr) {
-			DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
-		}
+	if (_distance_sensor_topic == nullptr) {
+		DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
 	}
 
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
 	_sensor_ok = true;
-out:
+
 	return ret;
 }
 

src/drivers/px4io/px4io.cpp

@@ -2090,12 +2090,12 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 
 		} while (buflen > 0);
 
-		/* ensure a closing newline */
+		int ret;
+
+		/* send the closing newline */
 		msg->text[0] = '\n';
 		msg->text[1] = '\0';
 
-		int ret;
-
 		for (int i = 0; i < 30; i++) {
 			/* failed, but give it a 2nd shot */
 			ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
@@ -2108,27 +2108,24 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 			}
 		}
 
-		if (ret) {
-			return ret;
+		if (ret == 0) {
+			/* success, exit */
+			break;
 		}
 
 		retries--;
 
-		DEVICE_LOG("mixer sent");
+	} while (retries > 0);
 
-	} while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK)));
+	if (retries == 0) {
+		mavlink_and_console_log_info(_mavlink_fd, "[IO] mixer upload fail");
+		/* load must have failed for some reason */
+		return -EINVAL;
 
-	/* check for the mixer-OK flag */
-	if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
-		mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok");
-		return 0;
+	} else {
+		/* all went well, set the mixer ok flag */
+		return io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_MIXER_OK);
 	}
-
-	DEVICE_LOG("mixer rejected by IO");
-	mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
-
-	/* load must have failed for some reason */
-	return -EINVAL;
 }
 
 void

src/drivers/sf0x/sf0x.cpp

@@ -296,16 +296,14 @@ SF0X::init()
 
 		_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
 
-		if (_class_instance == CLASS_DEVICE_PRIMARY) {
-			/* get a publish handle on the range finder topic */
-			struct distance_sensor_s ds_report = {};
+		/* get a publish handle on the range finder topic */
+		struct distance_sensor_s ds_report = {};
 
-			_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
-						 &_orb_class_instance, ORB_PRIO_HIGH);
+		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+					 &_orb_class_instance, ORB_PRIO_HIGH);
 
-			if (_distance_sensor_topic == nullptr) {
-				DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
-			}
+		if (_distance_sensor_topic == nullptr) {
+			DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
 		}
 
 	} while (0);

src/drivers/srf02/srf02.cpp

@@ -254,7 +254,7 @@ SRF02::init()
 
 	/* do I2C init (and probe) first */
 	if (I2C::init() != OK) {
-		goto out;
+		return ret;
 	}
 
 	/* allocate basic report buffers */
@@ -264,21 +264,19 @@ SRF02::init()
 	set_address(_index_counter);		/* set I2c port to temp sonar i2c adress */
 
 	if (_reports == nullptr) {
-		goto out;
+		return ret;
 	}
 
 	_class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH);
 
-	if (_class_instance == CLASS_DEVICE_PRIMARY) {
-		/* get a publish handle on the range finder topic */
-		struct distance_sensor_s ds_report = {};
+	/* get a publish handle on the range finder topic */
+	struct distance_sensor_s ds_report = {};
 
-		_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
-					 &_orb_class_instance, ORB_PRIO_LOW);
+	_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report,
+				 &_orb_class_instance, ORB_PRIO_LOW);
 
-		if (_distance_sensor_topic == nullptr) {
-			DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
-		}
+	if (_distance_sensor_topic == nullptr) {
+		DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?");
 	}
 
 	// XXX we should find out why we need to wait 200 ms here
@@ -320,7 +318,7 @@ SRF02::init()
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
 	_sensor_ok = true;
-out:
+
 	return ret;
 }
 

src/firmware/posix/CMakeLists.txt

@@ -26,27 +26,37 @@ endif()
 
 add_custom_target(run_config
 	COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}"
-	"${config_sitl_viewer}" "${CMAKE_BINARY_DIR}"
+	"${config_sitl_viewer}" "${config_sitl_model}" "${CMAKE_BINARY_DIR}"
 	WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 	USES_TERMINAL
 	)
 add_dependencies(run_config mainapp)
 
-foreach(viewer jmavsim gazebo)
+foreach(viewer none jmavsim gazebo)
 	foreach(debugger none gdb lldb)
-		if (debugger STREQUAL "none")
-			set(_targ_name "${viewer}")
-		else()
-			set(_targ_name "${viewer}_${debugger}")
-		endif()
-		add_custom_target(${_targ_name}
-			COMMAND Tools/sitl_run.sh "${config_sitl_rcS}"
-			"${debugger}"
-			"${viewer}" "${CMAKE_BINARY_DIR}"
-			WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
-			USES_TERMINAL
-			)
-		add_dependencies(${_targ_name} mainapp)
+		foreach(model none iris vtol)
+			if (debugger STREQUAL "none")
+				if (model STREQUAL "none")
+					set(_targ_name "${viewer}")
+				else()
+					set(_targ_name "${viewer}_${model}")
+				endif()
+			else()
+				if (model STREQUAL "none")
+					set(_targ_name "${viewer}___${debugger}")
+				else()
+					set(_targ_name "${viewer}_${model}_${debugger}")
+				endif()
+			endif()
+			add_custom_target(${_targ_name}
+				COMMAND Tools/sitl_run.sh "${config_sitl_rcS}"
+				"${debugger}"
+				"${viewer}" "${model}" "${CMAKE_BINARY_DIR}"
+				WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+				USES_TERMINAL
+				)
+			add_dependencies(${_targ_name} mainapp)
+		endforeach()
 	endforeach()
 endforeach()
 

src/lib/external_lgpl/tecs/tecs.h

@@ -118,6 +118,9 @@ public:
 		return get_throttle_demand();
 	}
 
+	void reset_state() {
+		_states_initalized = false;
+	}
 
 	float get_pitch_demand() { return _pitch_dem; }
 

src/lib/mathlib/math/test/test.cpp

@@ -54,6 +54,46 @@ bool __EXPORT equal(float a, float b, float epsilon)
 	} else { return true; }
 }
 
+bool __EXPORT greater_than(float a, float b)
+{
+	if (a > b) {
+		return true;
+	} else {
+		printf("not a > b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
+		return false;
+	}
+}
+
+bool __EXPORT less_than(float a, float b)
+{
+	if (a < b) {
+		return true;
+	} else {
+		printf("not a < b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
+		return false;
+	}
+}
+
+bool __EXPORT greater_than_or_equal(float a, float b)
+{
+	if (a >= b) {
+		return true;
+	} else {
+		printf("not a >= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
+		return false;
+	}
+}
+
+bool __EXPORT less_than_or_equal(float a, float b)
+{
+	if (a <= b) {
+		return true;
+	} else {
+		printf("not a <= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
+		return false;
+	}
+}
+
 void __EXPORT float2SigExp(
 	const float &num,
 	float &sig,

src/lib/mathlib/math/test/test.hpp

@@ -44,6 +44,15 @@
 //#include <stdlib.h>
 
 bool equal(float a, float b, float eps = 1e-5);
+
+bool greater_than(float a, float b);
+
+bool less_than(float a, float b);
+
+bool greater_than_or_equal(float a, float b);
+
+bool less_than_or_equal(float a, float b);
+
 void float2SigExp(
 	const float &num,
 	float &sig,

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -57,6 +57,8 @@
  #include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vision_position_estimate.h>
+#include <uORB/topics/att_pos_mocap.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
@@ -118,6 +120,8 @@ private:
 
 	int		_sensors_sub = -1;
 	int		_params_sub = -1;
+	int		_vision_sub = -1;
+	int		_mocap_sub = -1;
 	int		_global_pos_sub = -1;
 	orb_advert_t	_att_pub = nullptr;
 	orb_advert_t	_ctrl_state_pub = nullptr;
@@ -125,16 +129,19 @@ private:
 	struct {
 		param_t	w_acc;
 		param_t	w_mag;
+		param_t	w_ext_hdg;
 		param_t	w_gyro_bias;
 		param_t	mag_decl;
 		param_t	mag_decl_auto;
 		param_t	acc_comp;
 		param_t	bias_max;
 		param_t vibe_thresh;
+		param_t	ext_hdg_mode;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	float		_w_accel = 0.0f;
 	float		_w_mag = 0.0f;
+	float		_w_ext_hdg = 0.0f;
 	float		_w_gyro_bias = 0.0f;
 	float		_mag_decl = 0.0f;
 	bool		_mag_decl_auto = false;
@@ -142,11 +149,18 @@ private:
 	float		_bias_max = 0.0f;
 	float		_vibration_warning_threshold = 1.0f;
 	hrt_abstime	_vibration_warning_timestamp = 0;
+	int		_ext_hdg_mode = 0;
 
 	Vector<3>	_gyro;
 	Vector<3>	_accel;
 	Vector<3>	_mag;
 
+	vision_position_estimate_s _vision = {};
+	Vector<3>	_vision_hdg;
+
+	att_pos_mocap_s _mocap = {};
+	Vector<3>	_mocap_hdg;
+
 	Quaternion	_q;
 	Vector<3>	_rates;
 	Vector<3>	_gyro_bias;
@@ -170,6 +184,7 @@ private:
 	bool		_data_good = false;
 	bool		_failsafe = false;
 	bool		_vibration_warning = false;
+	bool		_ext_hdg_good = false;
 
 	int		_mavlink_fd = -1;
 
@@ -198,12 +213,14 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
 
 	_params_handles.w_acc		= param_find("ATT_W_ACC");
 	_params_handles.w_mag		= param_find("ATT_W_MAG");
+	_params_handles.w_ext_hdg	= param_find("ATT_W_EXT_HDG");
 	_params_handles.w_gyro_bias	= param_find("ATT_W_GYRO_BIAS");
 	_params_handles.mag_decl	= param_find("ATT_MAG_DECL");
 	_params_handles.mag_decl_auto	= param_find("ATT_MAG_DECL_A");
 	_params_handles.acc_comp	= param_find("ATT_ACC_COMP");
 	_params_handles.bias_max	= param_find("ATT_BIAS_MAX");
 	_params_handles.vibe_thresh	= param_find("ATT_VIBE_THRESH");
+	_params_handles.ext_hdg_mode	= param_find("ATT_EXT_HDG_M");
 }
 
 /**
@@ -271,6 +288,10 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
 void AttitudeEstimatorQ::task_main()
 {
 	_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
+
+	_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
+	_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
+
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 
@@ -376,6 +397,47 @@ void AttitudeEstimatorQ::task_main()
 			}
 		}
 
+		// Update vision and motion capture heading
+		bool vision_updated = false;
+		orb_check(_vision_sub, &vision_updated);
+
+		bool mocap_updated = false;
+		orb_check(_mocap_sub, &mocap_updated);
+
+		if (vision_updated) {
+			orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision);
+			math::Quaternion q(_vision.q);
+
+			math::Matrix<3, 3> Rvis = q.to_dcm();
+			math::Vector<3> v(1.0f, 0.0f, 0.4f);
+
+			// Rvis is Rwr (robot respect to world) while v is respect to world.
+			// Hence Rvis must be transposed having (Rwr)' * Vw
+			// Rrw * Vw = vn. This way we have consistency
+			_vision_hdg = Rvis.transposed() * v;
+		}
+
+		if (mocap_updated) {
+			orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap);
+			math::Quaternion q(_mocap.q);
+			math::Matrix<3, 3> Rmoc = q.to_dcm();
+
+			math::Vector<3> v(1.0f, 0.0f, 0.4f);
+
+			// Rmoc is Rwr (robot respect to world) while v is respect to world.
+			// Hence Rmoc must be transposed having (Rwr)' * Vw
+			// Rrw * Vw = vn. This way we have consistency
+			_mocap_hdg = Rmoc.transposed() * v;
+		}
+
+		// Check for timeouts on data
+		if (_ext_hdg_mode == 1) {
+			_ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000);
+
+		} else if (_ext_hdg_mode == 2) {
+			_ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000);
+		}
+
 		bool gpos_updated;
 		orb_check(_global_pos_sub, &gpos_updated);
 
@@ -503,6 +565,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
 
 		param_get(_params_handles.w_acc, &_w_accel);
 		param_get(_params_handles.w_mag, &_w_mag);
+		param_get(_params_handles.w_ext_hdg, &_w_ext_hdg);
 		param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
 		float mag_decl_deg = 0.0f;
 		param_get(_params_handles.mag_decl, &mag_decl_deg);
@@ -515,6 +578,7 @@ void AttitudeEstimatorQ::update_parameters(bool force)
 		_acc_comp = acc_comp_int != 0;
 		param_get(_params_handles.bias_max, &_bias_max);
 		param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold);
+		param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode);
 	}
 }
 
@@ -570,12 +634,34 @@ bool AttitudeEstimatorQ::update(float dt)
 	// Angular rate of correction
 	Vector<3> corr;
 
-	// Magnetometer correction
-	// Project mag field vector to global frame and extract XY component
-	Vector<3> mag_earth = _q.conjugate(_mag);
-	float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
-	// Project magnetometer correction to body frame
-	corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
+	if (_ext_hdg_mode > 0 && _ext_hdg_good) {
+		if (_ext_hdg_mode == 1) {
+			// Vision heading correction
+			// Project heading to global frame and extract XY component
+			Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg);
+			float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
+			// Project correction to body frame
+			corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg;
+		}
+
+		if (_ext_hdg_mode == 2) {
+			// Mocap heading correction
+			// Project heading to global frame and extract XY component
+			Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg);
+			float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
+			// Project correction to body frame
+			corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg;
+		}
+	}
+
+	if (_ext_hdg_mode == 0  || !_ext_hdg_good) {
+		// Magnetometer correction
+		// Project mag field vector to global frame and extract XY component
+		Vector<3> mag_earth = _q.conjugate(_mag);
+		float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
+		// Project magnetometer correction to body frame
+		corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
+	}
 
 	// Accelerometer correction
 	// Project 'k' unit vector of earth frame to body frame

src/modules/attitude_estimator_q/attitude_estimator_q_params.c

@@ -61,6 +61,15 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
  */
 PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
 
+/**
+ * Complimentary filter external heading weight
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_FLOAT(ATT_W_EXT_HDG, 0.1f);
+
 /**
  * Complimentary filter gyroscope bias weight
  *
@@ -93,6 +102,17 @@ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
  */
 PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);
 
+/**
+ * External heading usage mode (from Motion capture/Vision)
+ * Set to 1 to use heading estimate from vision.
+ * Set to 2 to use heading from motion capture.
+ *
+ * @group Attitude Q estimator
+ * @min 0
+ * @max 2
+ */
+PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0);
+
 /**
  * Enable acceleration compensation based on GPS
  * velocity.

src/modules/commander/CMakeLists.txt

@@ -32,7 +32,7 @@
 ############################################################################
 set(MODULE_CFLAGS -Os)
 if(${OS} STREQUAL "nuttx")
-	list(APPEND MODULE_CFLAGS -Wframe-larger-than=2300)
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=2450)
 endif()
 px4_add_module(
 	MODULE modules__commander

src/modules/commander/commander.cpp

@@ -72,6 +72,7 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_command.h>
@@ -92,6 +93,7 @@
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 #include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/input_rc.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -149,7 +151,7 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage
 #define PRINT_INTERVAL	5000000
 #define PRINT_MODE_REJECT_INTERVAL	10000000
 
-#define INAIR_RESTART_HOLDOFF_INTERVAL	2000000
+#define INAIR_RESTART_HOLDOFF_INTERVAL	1000000
 
 #define HIL_ID_MIN 1000
 #define HIL_ID_MAX 1999
@@ -178,11 +180,13 @@ static volatile bool thread_should_exit = false;	/**< daemon exit flag */
 static volatile bool thread_running = false;		/**< daemon status flag */
 static int daemon_task;					/**< Handle of daemon task / thread */
 static bool need_param_autosave = false;		/**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */
+static bool _usb_telemetry_active = false;
 static hrt_abstime commander_boot_timestamp = 0;
 
 static unsigned int leds_counter;
 /* To remember when last notification was sent */
 static uint64_t last_print_mode_reject_time = 0;
+static uint64_t _inair_last_time = 0;
 
 static float eph_threshold = 5.0f;
 static float epv_threshold = 10.0f;
@@ -197,7 +201,7 @@ static struct home_position_s _home;
 static unsigned _last_mission_instance = 0;
 static manual_control_setpoint_s _last_sp_man;
 
-struct vtol_vehicle_status_s vtol_status;
+static struct vtol_vehicle_status_s vtol_status;
 
 /**
  * The daemon app only briefly exists to start
@@ -221,7 +225,7 @@ void usage(const char *reason);
  */
 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd,
 		    struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
-		    struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub);
+		    struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub);
 
 /**
  * Mainloop of commander.
@@ -246,6 +250,9 @@ void print_reject_arm(const char *msg);
 
 void print_status();
 
+transition_result_t check_navigation_state_machine(struct vehicle_status_s *status,
+		struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
+
 transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
 
 /**
@@ -253,7 +260,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed
 *		 time the vehicle is armed with a good GPS fix.
 **/
 static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
-					const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition);
+					const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
+					const vehicle_attitude_s &attitude);
 
 /**
  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
@@ -291,7 +299,7 @@ int commander_main(int argc, char *argv[])
 		daemon_task = px4_task_spawn_cmd("commander",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_MAX - 40,
-					     3400,
+					     3500,
 					     commander_thread_main,
 					     (char * const *)&argv[0]);
 
@@ -396,11 +404,11 @@ int commander_main(int argc, char *argv[])
 
 void usage(const char *reason)
 {
-	if (reason) {
-		PX4_INFO("%s\n", reason);
+	if (reason && *reason > 0) {
+		PX4_INFO("%s", reason);
 	}
 
-	PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n");
+	PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n");
 }
 
 void print_status()
@@ -490,7 +498,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
 		    struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
 		    struct home_position_s *home, struct vehicle_global_position_s *global_pos,
-		    struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub)
+		    struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub)
 {
 	/* only handle commands that are meant to be handled by this system and component */
 	if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
@@ -524,7 +532,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			if (cmd_arm && (arming_ret == TRANSITION_CHANGED) &&
 				(hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) {
 
-				commander_set_home_position(*home_pub, *home, *local_pos, *global_pos);
+				commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude);
 			}
 
 			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
@@ -833,7 +841,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 *		 time the vehicle is armed with a good GPS fix.
 **/
 static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
-					const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition)
+					const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition,
+					const vehicle_attitude_s &attitude)
 {
 	//Need global position fix to be able to set home
 	if (!status.condition_global_position_valid) {
@@ -855,6 +864,8 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
 	home.y = localPosition.y;
 	home.z = localPosition.z;
 
+	home.yaw = attitude.yaw;
+
 	warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
 	mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
 
@@ -881,6 +892,7 @@ int commander_thread_main(int argc, char *argv[])
 	commander_initialized = false;
 
 	bool arm_tune_played = false;
+	bool was_landed = true;
 	bool was_armed = false;
 
 	bool startup_in_hil = false;
@@ -917,6 +929,8 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_eph = param_find("COM_HOME_H_T");
 	param_t _param_epv = param_find("COM_HOME_V_T");
 	param_t _param_geofence_action = param_find("GF_ACTION");
+	param_t _param_disarm_land = param_find("COM_DISARM_LAND");
+	param_t _param_map_mode_sw = param_find("RC_MAP_MODE_SW");
 
 	// const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
 	// main_states_str[vehicle_status_s::MAIN_STATE_MANUAL]			= "MANUAL";
@@ -941,6 +955,7 @@ int commander_thread_main(int argc, char *argv[])
 	const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX];
 	nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL]			= "MANUAL";
 	nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB]				= "STAB";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_RATTITUDE]		= "RATTITUDE";
 	nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL]			= "ALTCTL";
 	nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL]			= "POSCTL";
 	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
@@ -1029,17 +1044,15 @@ int commander_thread_main(int argc, char *argv[])
 		px4_task_exit(ERROR);
 	}
 
-	/* armed topic */
-	orb_advert_t armed_pub;
 	/* Initialize armed with all false */
 	memset(&armed, 0, sizeof(armed));
+	/* armed topic */
+	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
 
 	/* vehicle control mode topic */
 	memset(&control_mode, 0, sizeof(control_mode));
 	orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
 
-	armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
-
 	/* home position */
 	orb_advert_t home_pub = nullptr;
 	memset(&_home, 0, sizeof(_home));
@@ -1138,13 +1151,15 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* Subscribe to local position data */
 	int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	struct vehicle_local_position_s local_position;
-	memset(&local_position, 0, sizeof(local_position));
+	struct vehicle_local_position_s local_position = {};
+
+	/* Subscribe to attitude data */
+	int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	struct vehicle_attitude_s attitude = {};
 
 	/* Subscribe to land detector */
 	int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
-	struct vehicle_land_detected_s land_detector;
-	memset(&land_detector, 0, sizeof(land_detector));
+	struct vehicle_land_detected_s land_detector = {};
 
 	/*
 	 * The home position is set based on GPS only, to prevent a dependency between
@@ -1255,7 +1270,7 @@ int commander_thread_main(int argc, char *argv[])
 	float rc_loss_timeout = 0.5;
 	int32_t datalink_regain_timeout = 0;
 
-	uint8_t geofence_action = 0;
+	int32_t geofence_action = 0;
 
 	/* Thresholds for engine failure detection */
 	int32_t ef_throttle_thres = 1.0f;
@@ -1265,6 +1280,9 @@ int commander_thread_main(int argc, char *argv[])
 
 	int autosave_params; /**< Autosave of parameters enabled/disabled, loaded from parameter */
 
+	int32_t disarm_when_landed = 0;
+	int32_t map_mode_sw = 0;
+
 	/* check which state machines for changes, clear "changed" flag */
 	bool arming_state_changed = false;
 	bool main_state_changed = false;
@@ -1329,6 +1347,16 @@ int commander_thread_main(int argc, char *argv[])
 
 				status_changed = true;
 
+				// check if main mode switch has been assigned and if so run preflight checks in order
+				// to update status.condition_sensors_initialised
+				int32_t map_mode_sw_new;
+				param_get(_param_map_mode_sw, &map_mode_sw_new);
+
+				if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) {
+					status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
+							!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
+				}
+
 				/* re-check RC calibration */
 				rc_calibration_check(mavlink_fd);
 			}
@@ -1344,6 +1372,8 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
 			param_get(_param_ef_time_thres, &ef_time_thres);
 			param_get(_param_geofence_action, &geofence_action);
+			param_get(_param_disarm_land, &disarm_when_landed);
+			param_get(_param_map_mode_sw, &map_mode_sw);
 
 			/* Autostart id */
 			param_get(_param_autostart_id, &autostart_id);
@@ -1434,6 +1464,11 @@ int commander_thread_main(int argc, char *argv[])
 					}
 				}
 
+				/* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */
+				if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) {
+					_usb_telemetry_active = true;
+				}
+
 				telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
 			}
 		}
@@ -1489,7 +1524,20 @@ int commander_thread_main(int argc, char *argv[])
 
 				/* copy avionics voltage */
 				status.avionics_power_rail_voltage = system_power.voltage5V_v;
-				status.usb_connected = system_power.usb_connected;
+
+				/* if the USB hardware connection went away, reboot */
+				if (status.usb_connected && !system_power.usb_connected) {
+					/*
+					 * apparently the USB cable went away but we are still powered,
+					 * so lets reset to a classic non-usb state.
+					 */
+					mavlink_log_critical(mavlink_fd, "USB disconnected, rebooting.")
+					usleep(400000);
+					px4_systemreset(false);
+				}
+
+				/* finally judge the USB connected state based on software detection */
+				status.usb_connected = _usb_telemetry_active;
 			}
 		}
 
@@ -1574,6 +1622,14 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
 		}
 
+		/* update attitude estimate */
+		orb_check(attitude_sub, &updated);
+
+		if (updated) {
+			/* position changed */
+			orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude);
+		}
+
 		//update condition_global_position_valid
 		//Global positions are only published by the estimators if they are valid
 		if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) {
@@ -1618,12 +1674,13 @@ int commander_thread_main(int argc, char *argv[])
 			    &(status.condition_local_altitude_valid), &status_changed);
 
 		/* Update land detector */
+		static bool check_for_disarming = false;
 		orb_check(land_detector_sub, &updated);
 		if (updated) {
 			orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
 		}
 
-		if (updated && status.condition_local_altitude_valid) {
+		if ((updated && status.condition_local_altitude_valid) || check_for_disarming) {
 			if (status.condition_landed != land_detector.landed) {
 				status.condition_landed = land_detector.landed;
 				status_changed = true;
@@ -1635,6 +1692,24 @@ int commander_thread_main(int argc, char *argv[])
 					mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED");
 				}
 			}
+
+			if (disarm_when_landed > 0) {
+				if (land_detector.landed) {
+					if (!check_for_disarming && _inair_last_time > 0) {
+						_inair_last_time = land_detector.timestamp;
+						check_for_disarming = true;
+					}
+
+					if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) {
+						mavlink_log_critical(mavlink_fd, "AUTO DISARMING AFTER LANDING");
+						arm_disarm(false, mavlink_fd, "auto disarm on land");
+						_inair_last_time = 0;
+						check_for_disarming = false;
+					}
+				} else {
+					_inair_last_time = land_detector.timestamp;
+				}
+			}
 		}
 
 		/* update battery status */
@@ -1978,13 +2053,14 @@ int commander_thread_main(int argc, char *argv[])
 
 			status.rc_signal_lost = false;
 
-			/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
+			/* check if left stick is in lower left position and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) -> disarm
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF &&
 			    (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
 			    (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL ||
 			    	status.main_state == vehicle_status_s::MAIN_STATE_ACRO ||
 			    	status.main_state == vehicle_status_s::MAIN_STATE_STAB ||
+			    	status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE ||
 			    	status.condition_landed) &&
 			    sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 
@@ -2184,7 +2260,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
 
 			/* handle it */
-			if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub)) {
+			if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &attitude, &home_pub)) {
 				status_changed = true;
 			}
 		}
@@ -2196,6 +2272,7 @@ int commander_thread_main(int argc, char *argv[])
 			 * and both failed we want to terminate the flight */
 			if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL &&
 			    status.main_state !=vehicle_status_s::MAIN_STATE_ACRO &&
+			    status.main_state !=vehicle_status_s::MAIN_STATE_RATTITUDE &&
 			    status.main_state !=vehicle_status_s::MAIN_STATE_STAB &&
 			    status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL &&
 			    status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL &&
@@ -2220,6 +2297,7 @@ int commander_thread_main(int argc, char *argv[])
 			 * If we are in manual (controlled with RC):
 			 * if both failed we want to terminate the flight */
 			if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
+			     status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE ||
 			     status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
 			     status.main_state !=vehicle_status_s::MAIN_STATE_STAB ||
 			     status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
@@ -2246,14 +2324,18 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* First time home position update - but only if disarmed */
 		if (!status.condition_home_position_valid && !armed.armed) {
-			commander_set_home_position(home_pub, _home, local_position, global_position);
+			commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
 		}
 
-		/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
-		else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) {
-			commander_set_home_position(home_pub, _home, local_position, global_position);
+		/* update home position on arming if at least 1s from commander start spent to avoid setting home on in-air restart */
+		else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) &&
+			(now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) {
+			commander_set_home_position(home_pub, _home, local_position, global_position, attitude);
 		}
 
+		was_landed = status.condition_landed;
+		was_armed = armed.armed;
+
 		/* print new state */
 		if (arming_state_changed) {
 			status_changed = true;
@@ -2261,8 +2343,6 @@ int commander_thread_main(int argc, char *argv[])
 			arming_state_changed = false;
 		}
 
-		was_armed = armed.armed;
-
 		/* now set navigation state according to failsafe and main state */
 		bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
 						       mission_result.finished,
@@ -2518,6 +2598,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 		 (_last_sp_man.return_switch == sp_man->return_switch) &&
 		 (_last_sp_man.mode_switch == sp_man->mode_switch) &&
 		 (_last_sp_man.acro_switch == sp_man->acro_switch) &&
+		 (_last_sp_man.rattitude_switch == sp_man->rattitude_switch) &&
 		 (_last_sp_man.posctl_switch == sp_man->posctl_switch) &&
 		 (_last_sp_man.loiter_switch == sp_man->loiter_switch))) {
 
@@ -2585,7 +2666,16 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 				res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB);
 			}
 
-		} else {
+		} 
+		else if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){
+			/* Similar to acro transitions for multirotors.  FW aircraft don't need a 
+			 * rattitude mode.*/
+			if (status.is_rotary_wing) {
+				res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE);
+			} else {
+				res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB);
+			}
+		}else {
 			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
 		}
 
@@ -2708,6 +2798,18 @@ set_control_mode()
 		control_mode.flag_external_manual_override_ok = false;
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_RATTITUDE:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
 	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
@@ -2956,11 +3058,10 @@ void *commander_low_prio_loop(void *arg)
 				if (need_param_autosave_timeout > 0 && hrt_elapsed_time(&need_param_autosave_timeout) > 200000ULL) {
 					int ret = param_save_default();
 
-					if (ret == OK) {
-						mavlink_and_console_log_info(mavlink_fd, "settings autosaved");
-
+					if (ret != OK) {
+						mavlink_and_console_log_critical(mavlink_fd, "settings auto save error");
 					} else {
-						mavlink_and_console_log_critical(mavlink_fd, "settings save error");
+						warnx("settings saved.");
 					}
 
 					need_param_autosave = false;

src/modules/commander/commander_params.c

@@ -274,3 +274,16 @@ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1);
  * @max 2
  */
 PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0);
+
+/**
+ * Time-out for auto disarm after landing
+ *
+ * A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be
+ * automatically disarmed in case a landing situation has been detected during this period.
+ * A value of zero means that automatic disarming is disabled.
+ *
+ * @group Commander
+ * @min 0
+ */
+PARAM_DEFINE_INT32(COM_DISARM_LAND, 0);
+

src/modules/commander/state_machine_helper.cpp

@@ -304,6 +304,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	switch (new_main_state) {
 	case vehicle_status_s::MAIN_STATE_MANUAL:
 	case vehicle_status_s::MAIN_STATE_ACRO:
+	case vehicle_status_s::MAIN_STATE_RATTITUDE:
 	case vehicle_status_s::MAIN_STATE_STAB:
 		ret = TRANSITION_CHANGED;
 		break;
@@ -528,6 +529,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 	switch (status->main_state) {
 	case vehicle_status_s::MAIN_STATE_ACRO:
 	case vehicle_status_s::MAIN_STATE_MANUAL:
+	case vehicle_status_s::MAIN_STATE_RATTITUDE:
 	case vehicle_status_s::MAIN_STATE_STAB:
 	case vehicle_status_s::MAIN_STATE_ALTCTL:
 	case vehicle_status_s::MAIN_STATE_POSCTL:
@@ -555,6 +557,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 				break;
 
+			case vehicle_status_s::MAIN_STATE_RATTITUDE:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE;
+				break;
+
 			case vehicle_status_s::MAIN_STATE_STAB:
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
 				break;
@@ -737,5 +743,12 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 		checkAirspeed = true;
 	}
 
-	return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true);
+	bool prearm_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true);
+
+	if (status->usb_connected) {
+		prearm_ok = false;
+		mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
+	}
+
+	return !prearm_ok;
 }

src/modules/controllib/CMakeLists.txt

@@ -32,9 +32,11 @@
 ############################################################################
 px4_add_module(
 	MODULE modules__controllib
+	MAIN controllib_test
 	COMPILE_FLAGS
 		-Os
 	SRCS
+		controllib_test_main.cpp
 		test_params.c
 		block/Block.cpp
 		block/BlockParam.cpp

src/modules/controllib/blocks.cpp

@@ -43,6 +43,8 @@
 
 #include "blocks.hpp"
 
+#define ASSERT_CL(T) if (!(T)) { printf("FAIL\n"); return -1; }
+
 namespace control
 {
 
@@ -62,7 +64,8 @@ int basicBlocksTest()
 	blockPIDTest();
 	blockOutputTest();
 	blockRandUniformTest();
-	blockRandGaussTest();
+	// known failures
+	// blockRandGaussTest();
 	return 0;
 }
 
@@ -83,13 +86,13 @@ int blockLimitTest()
 	printf("Test BlockLimit\t\t\t: ");
 	BlockLimit limit(NULL, "TEST");
 	// initial state
-	ASSERT(equal(1.0f, limit.getMax()));
-	ASSERT(equal(-1.0f, limit.getMin()));
-	ASSERT(equal(0.0f, limit.getDt()));
+	ASSERT_CL(equal(1.0f, limit.getMax()));
+	ASSERT_CL(equal(-1.0f, limit.getMin()));
+	ASSERT_CL(equal(0.0f, limit.getDt()));
 	// update
-	ASSERT(equal(-1.0f, limit.update(-2.0f)));
-	ASSERT(equal(1.0f, limit.update(2.0f)));
-	ASSERT(equal(0.0f, limit.update(0.0f)));
+	ASSERT_CL(equal(-1.0f, limit.update(-2.0f)));
+	ASSERT_CL(equal(1.0f, limit.update(2.0f)));
+	ASSERT_CL(equal(0.0f, limit.update(0.0f)));
 	printf("PASS\n");
 	return 0;
 }
@@ -111,12 +114,12 @@ int blockLimitSymTest()
 	printf("Test BlockLimitSym\t\t: ");
 	BlockLimitSym limit(NULL, "TEST");
 	// initial state
-	ASSERT(equal(1.0f, limit.getMax()));
-	ASSERT(equal(0.0f, limit.getDt()));
+	ASSERT_CL(equal(1.0f, limit.getMax()));
+	ASSERT_CL(equal(0.0f, limit.getDt()));
 	// update
-	ASSERT(equal(-1.0f, limit.update(-2.0f)));
-	ASSERT(equal(1.0f, limit.update(2.0f)));
-	ASSERT(equal(0.0f, limit.update(0.0f)));
+	ASSERT_CL(equal(-1.0f, limit.update(-2.0f)));
+	ASSERT_CL(equal(1.0f, limit.update(2.0f)));
+	ASSERT_CL(equal(0.0f, limit.update(0.0f)));
 	printf("PASS\n");
 	return 0;
 }
@@ -138,25 +141,25 @@ int blockLowPassTest()
 	printf("Test BlockLowPass\t\t: ");
 	BlockLowPass lowPass(NULL, "TEST_LP");
 	// test initial state
-	ASSERT(equal(10.0f, lowPass.getFCut()));
-	ASSERT(equal(0.0f, lowPass.getState()));
-	ASSERT(equal(0.0f, lowPass.getDt()));
+	ASSERT_CL(equal(10.0f, lowPass.getFCut()));
+	ASSERT_CL(equal(0.0f, lowPass.getState()));
+	ASSERT_CL(equal(0.0f, lowPass.getDt()));
 	// set dt
 	lowPass.setDt(0.1f);
-	ASSERT(equal(0.1f, lowPass.getDt()));
+	ASSERT_CL(equal(0.1f, lowPass.getDt()));
 	// set state
 	lowPass.setState(1.0f);
-	ASSERT(equal(1.0f, lowPass.getState()));
+	ASSERT_CL(equal(1.0f, lowPass.getState()));
 	// test update
-	ASSERT(equal(1.8626974f, lowPass.update(2.0f)));
+	ASSERT_CL(equal(1.8626974f, lowPass.update(2.0f)));
 
 	// test end condition
 	for (int i = 0; i < 100; i++) {
 		lowPass.update(2.0f);
 	}
 
-	ASSERT(equal(2.0f, lowPass.getState()));
-	ASSERT(equal(2.0f, lowPass.update(2.0f)));
+	ASSERT_CL(equal(2.0f, lowPass.getState()));
+	ASSERT_CL(equal(2.0f, lowPass.update(2.0f)));
 	printf("PASS\n");
 	return 0;
 };
@@ -175,28 +178,28 @@ int blockHighPassTest()
 	printf("Test BlockHighPass\t\t: ");
 	BlockHighPass highPass(NULL, "TEST_HP");
 	// test initial state
-	ASSERT(equal(10.0f, highPass.getFCut()));
-	ASSERT(equal(0.0f, highPass.getU()));
-	ASSERT(equal(0.0f, highPass.getY()));
-	ASSERT(equal(0.0f, highPass.getDt()));
+	ASSERT_CL(equal(10.0f, highPass.getFCut()));
+	ASSERT_CL(equal(0.0f, highPass.getU()));
+	ASSERT_CL(equal(0.0f, highPass.getY()));
+	ASSERT_CL(equal(0.0f, highPass.getDt()));
 	// set dt
 	highPass.setDt(0.1f);
-	ASSERT(equal(0.1f, highPass.getDt()));
+	ASSERT_CL(equal(0.1f, highPass.getDt()));
 	// set state
 	highPass.setU(1.0f);
-	ASSERT(equal(1.0f, highPass.getU()));
+	ASSERT_CL(equal(1.0f, highPass.getU()));
 	highPass.setY(1.0f);
-	ASSERT(equal(1.0f, highPass.getY()));
+	ASSERT_CL(equal(1.0f, highPass.getY()));
 	// test update
-	ASSERT(equal(0.2746051f, highPass.update(2.0f)));
+	ASSERT_CL(equal(0.2746051f, highPass.update(2.0f)));
 
 	// test end condition
 	for (int i = 0; i < 100; i++) {
 		highPass.update(2.0f);
 	}
 
-	ASSERT(equal(0.0f, highPass.getY()));
-	ASSERT(equal(0.0f, highPass.update(2.0f)));
+	ASSERT_CL(equal(0.0f, highPass.getY()));
+	ASSERT_CL(equal(0.0f, highPass.update(2.0f)));
 	printf("PASS\n");
 	return 0;
 }
@@ -220,25 +223,25 @@ int blockLowPass2Test()
 	printf("Test BlockLowPass2\t\t: ");
 	BlockLowPass2 lowPass(NULL, "TEST_LP", 100);
 	// test initial state
-	ASSERT(equal(10.0f, lowPass.getFCutParam()));
-	ASSERT(equal(0.0f, lowPass.getState()));
-	ASSERT(equal(0.0f, lowPass.getDt()));
+	ASSERT_CL(equal(10.0f, lowPass.getFCutParam()));
+	ASSERT_CL(equal(0.0f, lowPass.getState()));
+	ASSERT_CL(equal(0.0f, lowPass.getDt()));
 	// set dt
 	lowPass.setDt(0.1f);
-	ASSERT(equal(0.1f, lowPass.getDt()));
+	ASSERT_CL(equal(0.1f, lowPass.getDt()));
 	// set state
 	lowPass.setState(1.0f);
-	ASSERT(equal(1.0f, lowPass.getState()));
+	ASSERT_CL(equal(1.0f, lowPass.getState()));
 	// test update
-	ASSERT(equal(1.06745527f, lowPass.update(2.0f)));
+	ASSERT_CL(equal(1.06745527f, lowPass.update(2.0f)));
 
 	// test end condition
 	for (int i = 0; i < 100; i++) {
 		lowPass.update(2.0f);
 	}
 
-	ASSERT(equal(2.0f, lowPass.getState()));
-	ASSERT(equal(2.0f, lowPass.update(2.0f)));
+	ASSERT_CL(equal(2.0f, lowPass.getState()));
+	ASSERT_CL(equal(2.0f, lowPass.update(2.0f)));
 	printf("PASS\n");
 	return 0;
 };
@@ -255,34 +258,34 @@ int blockIntegralTest()
 	printf("Test BlockIntegral\t\t: ");
 	BlockIntegral integral(NULL, "TEST_I");
 	// test initial state
-	ASSERT(equal(1.0f, integral.getMax()));
-	ASSERT(equal(0.0f, integral.getDt()));
+	ASSERT_CL(equal(1.0f, integral.getMax()));
+	ASSERT_CL(equal(0.0f, integral.getDt()));
 	// set dt
 	integral.setDt(0.1f);
-	ASSERT(equal(0.1f, integral.getDt()));
+	ASSERT_CL(equal(0.1f, integral.getDt()));
 	// set Y
 	integral.setY(0.9f);
-	ASSERT(equal(0.9f, integral.getY()));
+	ASSERT_CL(equal(0.9f, integral.getY()));
 
 	// test exceed max
 	for (int i = 0; i < 100; i++) {
 		integral.update(1.0f);
 	}
 
-	ASSERT(equal(1.0f, integral.update(1.0f)));
+	ASSERT_CL(equal(1.0f, integral.update(1.0f)));
 	// test exceed min
 	integral.setY(-0.9f);
-	ASSERT(equal(-0.9f, integral.getY()));
+	ASSERT_CL(equal(-0.9f, integral.getY()));
 
 	for (int i = 0; i < 100; i++) {
 		integral.update(-1.0f);
 	}
 
-	ASSERT(equal(-1.0f, integral.update(-1.0f)));
+	ASSERT_CL(equal(-1.0f, integral.update(-1.0f)));
 	// test update
 	integral.setY(0.1f);
-	ASSERT(equal(0.2f, integral.update(1.0)));
-	ASSERT(equal(0.2f, integral.getY()));
+	ASSERT_CL(equal(0.2f, integral.update(1.0)));
+	ASSERT_CL(equal(0.2f, integral.getY()));
 	printf("PASS\n");
 	return 0;
 }
@@ -301,40 +304,40 @@ int blockIntegralTrapTest()
 	printf("Test BlockIntegralTrap\t\t: ");
 	BlockIntegralTrap integral(NULL, "TEST_I");
 	// test initial state
-	ASSERT(equal(1.0f, integral.getMax()));
-	ASSERT(equal(0.0f, integral.getDt()));
+	ASSERT_CL(equal(1.0f, integral.getMax()));
+	ASSERT_CL(equal(0.0f, integral.getDt()));
 	// set dt
 	integral.setDt(0.1f);
-	ASSERT(equal(0.1f, integral.getDt()));
+	ASSERT_CL(equal(0.1f, integral.getDt()));
 	// set U
 	integral.setU(1.0f);
-	ASSERT(equal(1.0f, integral.getU()));
+	ASSERT_CL(equal(1.0f, integral.getU()));
 	// set Y
 	integral.setY(0.9f);
-	ASSERT(equal(0.9f, integral.getY()));
+	ASSERT_CL(equal(0.9f, integral.getY()));
 
 	// test exceed max
 	for (int i = 0; i < 100; i++) {
 		integral.update(1.0f);
 	}
 
-	ASSERT(equal(1.0f, integral.update(1.0f)));
+	ASSERT_CL(equal(1.0f, integral.update(1.0f)));
 	// test exceed min
 	integral.setU(-1.0f);
 	integral.setY(-0.9f);
-	ASSERT(equal(-0.9f, integral.getY()));
+	ASSERT_CL(equal(-0.9f, integral.getY()));
 
 	for (int i = 0; i < 100; i++) {
 		integral.update(-1.0f);
 	}
 
-	ASSERT(equal(-1.0f, integral.update(-1.0f)));
+	ASSERT_CL(equal(-1.0f, integral.update(-1.0f)));
 	// test update
 	integral.setU(2.0f);
 	integral.setY(0.1f);
-	ASSERT(equal(0.25f, integral.update(1.0)));
-	ASSERT(equal(0.25f, integral.getY()));
-	ASSERT(equal(1.0f, integral.getU()));
+	ASSERT_CL(equal(0.25f, integral.update(1.0)));
+	ASSERT_CL(equal(0.25f, integral.getY()));
+	ASSERT_CL(equal(1.0f, integral.getU()));
 	printf("PASS\n");
 	return 0;
 }
@@ -352,6 +355,7 @@ float BlockDerivative::update(float input)
 		// and so we use the assumption the
 		// input value is not changing much,
 		// which is the best we can do here.
+		_lowPass.update(0.0f);
 		output = 0.0f;
 		_initialized = true;
 	}
@@ -365,17 +369,20 @@ int blockDerivativeTest()
 	printf("Test BlockDerivative\t\t: ");
 	BlockDerivative derivative(NULL, "TEST_D");
 	// test initial state
-	ASSERT(equal(0.0f, derivative.getU()));
-	ASSERT(equal(10.0f, derivative.getLP()));
+	ASSERT_CL(equal(0.0f, derivative.getU()));
+	ASSERT_CL(equal(10.0f, derivative.getLP()));
 	// set dt
 	derivative.setDt(0.1f);
-	ASSERT(equal(0.1f, derivative.getDt()));
+	ASSERT_CL(equal(0.1f, derivative.getDt()));
 	// set U
 	derivative.setU(1.0f);
-	ASSERT(equal(1.0f, derivative.getU()));
+	ASSERT_CL(equal(1.0f, derivative.getU()));
+	// perform one update so initialized is set
+	derivative.update(1.0);
+	ASSERT_CL(equal(1.0f, derivative.getU()));
 	// test  update
-	ASSERT(equal(8.6269744f, derivative.update(2.0f)));
-	ASSERT(equal(2.0f, derivative.getU()));
+	ASSERT_CL(equal(8.6269744f, derivative.update(2.0f)));
+	ASSERT_CL(equal(2.0f, derivative.getU()));
 	printf("PASS\n");
 	return 0;
 }
@@ -385,13 +392,13 @@ int blockPTest()
 	printf("Test BlockP\t\t\t: ");
 	BlockP blockP(NULL, "TEST_P");
 	// test initial state
-	ASSERT(equal(0.2f, blockP.getKP()));
-	ASSERT(equal(0.0f, blockP.getDt()));
+	ASSERT_CL(equal(0.2f, blockP.getKP()));
+	ASSERT_CL(equal(0.0f, blockP.getDt()));
 	// set dt
 	blockP.setDt(0.1f);
-	ASSERT(equal(0.1f, blockP.getDt()));
+	ASSERT_CL(equal(0.1f, blockP.getDt()));
 	// test  update
-	ASSERT(equal(0.4f, blockP.update(2.0f)));
+	ASSERT_CL(equal(0.4f, blockP.update(2.0f)));
 	printf("PASS\n");
 	return 0;
 }
@@ -401,19 +408,19 @@ int blockPITest()
 	printf("Test BlockPI\t\t\t: ");
 	BlockPI blockPI(NULL, "TEST");
 	// test initial state
-	ASSERT(equal(0.2f, blockPI.getKP()));
-	ASSERT(equal(0.1f, blockPI.getKI()));
-	ASSERT(equal(0.0f, blockPI.getDt()));
-	ASSERT(equal(1.0f, blockPI.getIntegral().getMax()));
+	ASSERT_CL(equal(0.2f, blockPI.getKP()));
+	ASSERT_CL(equal(0.1f, blockPI.getKI()));
+	ASSERT_CL(equal(0.0f, blockPI.getDt()));
+	ASSERT_CL(equal(1.0f, blockPI.getIntegral().getMax()));
 	// set dt
 	blockPI.setDt(0.1f);
-	ASSERT(equal(0.1f, blockPI.getDt()));
+	ASSERT_CL(equal(0.1f, blockPI.getDt()));
 	// set integral state
 	blockPI.getIntegral().setY(0.1f);
-	ASSERT(equal(0.1f, blockPI.getIntegral().getY()));
+	ASSERT_CL(equal(0.1f, blockPI.getIntegral().getY()));
 	// test  update
 	// 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43
-	ASSERT(equal(0.43f, blockPI.update(2.0f)));
+	ASSERT_CL(equal(0.43f, blockPI.update(2.0f)));
 	printf("PASS\n");
 	return 0;
 }
@@ -423,19 +430,22 @@ int blockPDTest()
 	printf("Test BlockPD\t\t\t: ");
 	BlockPD blockPD(NULL, "TEST");
 	// test initial state
-	ASSERT(equal(0.2f, blockPD.getKP()));
-	ASSERT(equal(0.01f, blockPD.getKD()));
-	ASSERT(equal(0.0f, blockPD.getDt()));
-	ASSERT(equal(10.0f, blockPD.getDerivative().getLP()));
+	ASSERT_CL(equal(0.2f, blockPD.getKP()));
+	ASSERT_CL(equal(0.01f, blockPD.getKD()));
+	ASSERT_CL(equal(0.0f, blockPD.getDt()));
+	ASSERT_CL(equal(10.0f, blockPD.getDerivative().getLP()));
 	// set dt
 	blockPD.setDt(0.1f);
-	ASSERT(equal(0.1f, blockPD.getDt()));
+	ASSERT_CL(equal(0.1f, blockPD.getDt()));
 	// set derivative state
 	blockPD.getDerivative().setU(1.0f);
-	ASSERT(equal(1.0f, blockPD.getDerivative().getU()));
+	ASSERT_CL(equal(1.0f, blockPD.getDerivative().getU()));
+	// perform one update so initialized is set
+	blockPD.getDerivative().update(1.0);
+	ASSERT_CL(equal(1.0f, blockPD.getDerivative().getU()));
 	// test  update
 	// 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744
-	ASSERT(equal(0.486269744f, blockPD.update(2.0f)));
+	ASSERT_CL(equal(0.486269744f, blockPD.update(2.0f)));
 	printf("PASS\n");
 	return 0;
 }
@@ -445,24 +455,27 @@ int blockPIDTest()
 	printf("Test BlockPID\t\t\t: ");
 	BlockPID blockPID(NULL, "TEST");
 	// test initial state
-	ASSERT(equal(0.2f, blockPID.getKP()));
-	ASSERT(equal(0.1f, blockPID.getKI()));
-	ASSERT(equal(0.01f, blockPID.getKD()));
-	ASSERT(equal(0.0f, blockPID.getDt()));
-	ASSERT(equal(10.0f, blockPID.getDerivative().getLP()));
-	ASSERT(equal(1.0f, blockPID.getIntegral().getMax()));
+	ASSERT_CL(equal(0.2f, blockPID.getKP()));
+	ASSERT_CL(equal(0.1f, blockPID.getKI()));
+	ASSERT_CL(equal(0.01f, blockPID.getKD()));
+	ASSERT_CL(equal(0.0f, blockPID.getDt()));
+	ASSERT_CL(equal(10.0f, blockPID.getDerivative().getLP()));
+	ASSERT_CL(equal(1.0f, blockPID.getIntegral().getMax()));
 	// set dt
 	blockPID.setDt(0.1f);
-	ASSERT(equal(0.1f, blockPID.getDt()));
+	ASSERT_CL(equal(0.1f, blockPID.getDt()));
 	// set derivative state
 	blockPID.getDerivative().setU(1.0f);
-	ASSERT(equal(1.0f, blockPID.getDerivative().getU()));
+	ASSERT_CL(equal(1.0f, blockPID.getDerivative().getU()));
+	// perform one update so initialized is set
+	blockPID.getDerivative().update(1.0);
+	ASSERT_CL(equal(1.0f, blockPID.getDerivative().getU()));
 	// set integral state
 	blockPID.getIntegral().setY(0.1f);
-	ASSERT(equal(0.1f, blockPID.getIntegral().getY()));
+	ASSERT_CL(equal(0.1f, blockPID.getIntegral().getY()));
 	// test  update
 	// 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697
-	ASSERT(equal(0.5162697f, blockPID.update(2.0f)));
+	ASSERT_CL(equal(0.5162697f, blockPID.update(2.0f)));
 	printf("PASS\n");
 	return 0;
 }
@@ -472,19 +485,19 @@ int blockOutputTest()
 	printf("Test BlockOutput\t\t: ");
 	BlockOutput blockOutput(NULL, "TEST");
 	// test initial state
-	ASSERT(equal(0.0f, blockOutput.getDt()));
-	ASSERT(equal(0.5f, blockOutput.get()));
-	ASSERT(equal(-1.0f, blockOutput.getMin()));
-	ASSERT(equal(1.0f, blockOutput.getMax()));
+	ASSERT_CL(equal(0.0f, blockOutput.getDt()));
+	ASSERT_CL(equal(0.5f, blockOutput.get()));
+	ASSERT_CL(equal(-1.0f, blockOutput.getMin()));
+	ASSERT_CL(equal(1.0f, blockOutput.getMax()));
 	// test update below min
 	blockOutput.update(-2.0f);
-	ASSERT(equal(-1.0f, blockOutput.get()));
+	ASSERT_CL(equal(-1.0f, blockOutput.get()));
 	// test update above max
 	blockOutput.update(2.0f);
-	ASSERT(equal(1.0f, blockOutput.get()));
+	ASSERT_CL(equal(1.0f, blockOutput.get()));
 	// test trim
 	blockOutput.update(0.0f);
-	ASSERT(equal(0.5f, blockOutput.get()));
+	ASSERT_CL(equal(0.5f, blockOutput.get()));
 	printf("PASS\n");
 	return 0;
 }
@@ -495,23 +508,22 @@ int blockRandUniformTest()
 	printf("Test BlockRandUniform\t\t: ");
 	BlockRandUniform blockRandUniform(NULL, "TEST");
 	// test initial state
-	ASSERT(equal(0.0f, blockRandUniform.getDt()));
-	ASSERT(equal(-1.0f, blockRandUniform.getMin()));
-	ASSERT(equal(1.0f, blockRandUniform.getMax()));
+	ASSERT_CL(equal(0.0f, blockRandUniform.getDt()));
+	ASSERT_CL(equal(-1.0f, blockRandUniform.getMin()));
+	ASSERT_CL(equal(1.0f, blockRandUniform.getMax()));
 	// test update
 	int n = 10000;
 	float mean = blockRandUniform.update();
 
-	// recursive mean algorithm from Knuth
 	for (int i = 2; i < n + 1; i++) {
 		float val = blockRandUniform.update();
 		mean += (val - mean) / i;
-		ASSERT(val <= blockRandUniform.getMax());
-		ASSERT(val >= blockRandUniform.getMin());
+		ASSERT_CL(less_than_or_equal(val, blockRandUniform.getMax()));
+		ASSERT_CL(greater_than_or_equal(val, blockRandUniform.getMin()));
 	}
 
-	ASSERT(equal(mean, (blockRandUniform.getMin() +
-			    blockRandUniform.getMax()) / 2, 1e-1));
+	ASSERT_CL(equal(mean, (blockRandUniform.getMin() +
+			       blockRandUniform.getMax()) / 2, 1e-1));
 	printf("PASS\n");
 	return 0;
 }
@@ -522,9 +534,9 @@ int blockRandGaussTest()
 	printf("Test BlockRandGauss\t\t: ");
 	BlockRandGauss blockRandGauss(NULL, "TEST");
 	// test initial state
-	ASSERT(equal(0.0f, blockRandGauss.getDt()));
-	ASSERT(equal(1.0f, blockRandGauss.getMean()));
-	ASSERT(equal(2.0f, blockRandGauss.getStdDev()));
+	ASSERT_CL(equal(0.0f, blockRandGauss.getDt()));
+	ASSERT_CL(equal(1.0f, blockRandGauss.getMean()));
+	ASSERT_CL(equal(2.0f, blockRandGauss.getStdDev()));
 	// test update
 	int n = 10000;
 	float mean = blockRandGauss.update();
@@ -540,8 +552,8 @@ int blockRandGaussTest()
 
 	float stdDev = sqrt(sum / (n - 1));
 	(void)(stdDev);
-	ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1));
-	ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
+	ASSERT_CL(equal(mean, blockRandGauss.getMean(), 1e-1));
+	ASSERT_CL(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
 	printf("PASS\n");
 	return 0;
 }

src/modules/controllib/controllib_test_main.cpp

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file controllib.cpp
+ * Unit testing for controllib.
+ *
+ * @author James Goppert <james.goppert@gmail.com>
+ */
+
+#include "blocks.hpp"
+
+extern "C" __EXPORT int controllib_test_main(int argc, char *argv[]);
+
+int controllib_test_main(int argc, char *argv[])
+{
+	(void)argc;
+	(void)argv;
+	control::basicBlocksTest();
+	return 0;
+}

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -590,6 +590,11 @@ void AttitudePositionEstimatorEKF::task_main()
 				/* system is in HIL now, wait for measurements to come in one last round */
 				usleep(60000);
 
+				/* HIL is slow, set permissive timeouts */
+				_voter_gyro.set_timeout(500000);
+				_voter_accel.set_timeout(500000);
+				_voter_mag.set_timeout(500000);
+
 				/* now read all sensor publications to ensure all real sensor data is purged */
 				orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 
@@ -1295,6 +1300,23 @@ void AttitudePositionEstimatorEKF::pollData()
 
 	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
 
+	/* set time fields */
+	IMUusec = _sensor_combined.timestamp;
+	float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f;
+
+	/* guard against too large deltaT's */
+	if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.0001f) {
+
+		if (PX4_ISFINITE(_ekf->dtIMUfilt) && _ekf->dtIMUfilt < 0.5f && _ekf->dtIMUfilt > 0.0001f) {
+			deltaT = _ekf->dtIMUfilt;
+		} else {
+			deltaT = 0.01f;
+		}
+	}
+
+	/* fill in last data set */
+	_ekf->dtIMU = deltaT;
+
 	// Feed validator with recent sensor data
 
 	for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) {
@@ -1317,6 +1339,11 @@ void AttitudePositionEstimatorEKF::pollData()
 			_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
 			_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
 		} else {
+			float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f;
+			if (PX4_ISFINITE(dt_gyro) && (dt_gyro < 0.5f) && (dt_gyro > 0.00001f)) {
+				deltaT = dt_gyro;
+				_ekf->dtIMU = deltaT;
+			}
 			_ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU;
 			_ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU;
 			_ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU;
@@ -1389,20 +1416,8 @@ void AttitudePositionEstimatorEKF::pollData()
 		_vibration_warning_timestamp = 0;
 	}
 
-	IMUusec = _sensor_combined.timestamp;
-
-	float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f;
 	_last_sensor_timestamp = _sensor_combined.timestamp;
 
-	/* guard against too large deltaT's */
-	if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
-		deltaT = 0.01f;
-	}
-
-	// Always store data, independent of init status
-	/* fill in last data set */
-	_ekf->dtIMU = deltaT;
-
 	// XXX this is for assessing the filter performance
 	// leave this in as long as larger improvements are still being made.
 	#if 0
@@ -1414,7 +1429,7 @@ void AttitudePositionEstimatorEKF::pollData()
 	static unsigned dtoverflow10 = 0;
 	static hrt_abstime lastprint = 0;
 
-	if (hrt_elapsed_time(&lastprint) > 1000000) {
+	if (hrt_elapsed_time(&lastprint) > 1000000 || _sensor_combined.gyro_rad_s[0] > 4.0f) {
 		PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
 			(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
 			dtoverflow5, dtoverflow10);
@@ -1433,7 +1448,15 @@ void AttitudePositionEstimatorEKF::pollData()
 		(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
 		(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
 		(double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
-		(double)((_sensor_combined.accelerometer_m_s2[2] + 9.8665f) * deltaT));
+		(double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
+
+		PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f",
+				(double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed);
+
+		PX4_WARN("DRV rate: %8.4f, %8.4f, %8.4f",
+				(double)_sensor_combined.gyro_rad_s[0],
+				(double)_sensor_combined.gyro_rad_s[1],
+				(double)_sensor_combined.gyro_rad_s[2]);
 
 		lastprint = hrt_absolute_time();
 	}
@@ -1721,8 +1744,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
 			PX4_INFO(".");
 		}
 
-		PX4_INFO(" ");
-
 		return 0;
 	}
 

src/modules/ekf_att_pos_estimator/estimator_22states.cpp

@@ -2827,7 +2827,7 @@ bool AttPosEKF::VelNEDDiverged()
     Vector3f delta = current_vel - gps_vel;
     float delta_len = delta.length();
 
-    bool excessive = (delta_len > 20.0f);
+    bool excessive = (delta_len > 30.0f);
 
     current_ekf_state.error |= excessive;
     current_ekf_state.velOffsetExcessive = excessive;

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1102,6 +1102,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
 				_mTecs.resetDerivatives(_ctrl_state.airspeed);
+			} else {
+				_tecs.reset_state();
 			}
 		}
 		_control_mode_current = FW_POSCTRL_MODE_AUTO;
@@ -1461,6 +1463,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
 				_mTecs.resetDerivatives(_ctrl_state.airspeed);
+			} else {
+				_tecs.reset_state();
 			}
 		}
 		_control_mode_current = FW_POSCTRL_MODE_POSITION;
@@ -1572,6 +1576,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
 				_mTecs.resetDerivatives(_ctrl_state.airspeed);
+			} else {
+				_tecs.reset_state();
 			}
 		}
 		_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;

src/modules/mavlink/mavlink_main.cpp

@@ -698,6 +698,9 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 
 	} else {
 		_is_usb_uart = true;
+		/* USB has no baudrate, but use a magic number for 'fast' */
+		_baudrate = 2000000;
+		_rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB;
 	}
 
 #if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
@@ -2094,8 +2097,12 @@ Mavlink::display_status()
 			printf("3DR RADIO\n");
 			break;
 
+		case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB:
+			printf("USB CDC\n");
+			break;
+
 		default:
-			printf("UNKNOWN RADIO\n");
+			printf("GENERIC LINK OR RADIO\n");
 			break;
 		}
 

src/modules/mavlink/mavlink_main.h

@@ -288,6 +288,8 @@ public:
 
 	float			get_rate_mult();
 
+	float			get_baudrate() { return _baudrate; }
+
 	/* Functions for waiting to start transmission until message received. */
 	void			set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
 	bool			get_has_received_messages() { return _received_messages; }

src/modules/mavlink/mavlink_mission.cpp

@@ -422,7 +422,9 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
 		} else {
 			_mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch");
 
-			if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); }
+			if (_verbose) {
+				warnx("WPM: MISSION_ACK ERR: ID mismatch");
+			}
 		}
 	}
 }
@@ -473,13 +475,13 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
 		if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
 			_time_last_recv = hrt_absolute_time();
 
-			if (_count > 0) {
-				_state = MAVLINK_WPM_STATE_SENDLIST;
-				_transfer_seq = 0;
-				_transfer_count = _count;
-				_transfer_partner_sysid = msg->sysid;
-				_transfer_partner_compid = msg->compid;
+			_state = MAVLINK_WPM_STATE_SENDLIST;
+			_transfer_seq = 0;
+			_transfer_count = _count;
+			_transfer_partner_sysid = msg->sysid;
+			_transfer_partner_compid = msg->compid;
 
+			if (_count > 0) {
 				if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); }
 
 			} else {
@@ -598,9 +600,8 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 
 				/* alternate dataman ID anyway to let navigator know about changes */
 				update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0);
-				_mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION");
 
-				// TODO send ACK?
+				send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
 				_transfer_in_progress = false;
 				return;
 			}
@@ -712,8 +713,6 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
 			/* got all new mission items successfully */
 			if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); }
 
-			_mavlink->send_statustext_info("WPM: Transfer complete.");
-
 			_state = MAVLINK_WPM_STATE_IDLE;
 
 			if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) {

src/modules/mavlink/mavlink_receiver.cpp

@@ -1116,7 +1116,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 		/* yaw */
 		rc.values[2] = man.r / 2 + 1500;
 		/* throttle */
-		rc.values[3] = man.z / 1 + 1000;
+		rc.values[3] = fminf(fmaxf(man.z / 0.9f + 800, 1000.0f), 2000.0f);
 
 		/* decode all switches which fit into the channel mask */
 		unsigned max_switch = (sizeof(man.buttons) * 8);
@@ -1307,18 +1307,22 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 	float dt;
 
 	if (_hil_last_frame == 0 ||
-	   (imu.time_usec - _hil_last_frame) > (0.1f * 1e6f) ||
-	   (_hil_last_frame >= imu.time_usec)) {
+	   (timestamp <= _hil_last_frame) ||
+	   (timestamp - _hil_last_frame) > (0.1f * 1e6f) ||
+	   (_hil_last_frame >= timestamp)) {
 		dt = 0.01f; /* default to 100 Hz */
+		memset(&_hil_prev_gyro[0], 0, sizeof(_hil_prev_gyro));
+		_hil_prev_accel[0] = 0.0f;
+		_hil_prev_accel[1] = 0.0f;
+		_hil_prev_accel[2] = 9.866f;
 	} else {
-		dt = (imu.time_usec - _hil_last_frame) / 1e6f;
+		dt = (timestamp - _hil_last_frame) / 1e6f;
 	}
-	_hil_last_frame = imu.time_usec;
+	_hil_last_frame = timestamp;
 
 	/* airspeed */
 	{
-		struct airspeed_s airspeed;
-		memset(&airspeed, 0, sizeof(airspeed));
+		struct airspeed_s airspeed = {};
 
 		float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f);
 		// XXX need to fix this
@@ -1338,8 +1342,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 	/* gyro */
 	{
-		struct gyro_report gyro;
-		memset(&gyro, 0, sizeof(gyro));
+		struct gyro_report gyro = {};
 
 		gyro.timestamp = timestamp;
 		gyro.x_raw = imu.xgyro * 1000.0f;
@@ -1360,8 +1363,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 	/* accelerometer */
 	{
-		struct accel_report accel;
-		memset(&accel, 0, sizeof(accel));
+		struct accel_report accel = {};
 
 		accel.timestamp = timestamp;
 		accel.x_raw = imu.xacc / mg2ms2;
@@ -1382,8 +1384,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 	/* magnetometer */
 	{
-		struct mag_report mag;
-		memset(&mag, 0, sizeof(mag));
+		struct mag_report mag = {};
 
 		mag.timestamp = timestamp;
 		mag.x_raw = imu.xmag * 1000.0f;
@@ -1404,8 +1405,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 	/* baro */
 	{
-		struct baro_report baro;
-		memset(&baro, 0, sizeof(baro));
+		struct baro_report baro = {};
 
 		baro.timestamp = timestamp;
 		baro.pressure = imu.abs_pressure;
@@ -1422,21 +1422,20 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 	/* sensor combined */
 	{
-		struct sensor_combined_s hil_sensors;
-		memset(&hil_sensors, 0, sizeof(hil_sensors));
+		struct sensor_combined_s hil_sensors = {};
 
 		hil_sensors.timestamp = timestamp;
 
 		hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f;
 		hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f;
 		hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f;
-		hil_sensors.gyro_rad_s[0] = ((imu.xgyro * dt + _hil_prev_gyro[0]) / 2.0f) / dt;
+		hil_sensors.gyro_rad_s[0] = imu.xgyro;
 		hil_sensors.gyro_rad_s[1] = imu.ygyro;
 		hil_sensors.gyro_rad_s[2] = imu.zgyro;
-		hil_sensors.gyro_integral_rad[0] = (hil_sensors.gyro_rad_s[0] * dt + _hil_prev_gyro[0]) / 2.0f;
-		hil_sensors.gyro_integral_rad[1] = (hil_sensors.gyro_rad_s[1] * dt + _hil_prev_gyro[1]) / 2.0f;
-		hil_sensors.gyro_integral_rad[2] = (hil_sensors.gyro_rad_s[2] * dt + _hil_prev_gyro[2]) / 2.0f;
-		memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_integral_rad[0], sizeof(_hil_prev_gyro));
+		hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt;
+		hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt;
+		hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt;
+		memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_rad_s[0], sizeof(_hil_prev_gyro));
 		hil_sensors.gyro_integral_dt[0] = dt * 1e6f;
 		hil_sensors.gyro_timestamp[0] = timestamp;
 		hil_sensors.gyro_priority[0] = ORB_PRIO_HIGH;
@@ -1447,10 +1446,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.accelerometer_m_s2[0] = imu.xacc;
 		hil_sensors.accelerometer_m_s2[1] = imu.yacc;
 		hil_sensors.accelerometer_m_s2[2] = imu.zacc;
-		hil_sensors.accelerometer_integral_m_s[0] = (imu.xacc * dt + _hil_prev_accel[0]) / 2.0f;
-		hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f;
-		hil_sensors.accelerometer_integral_m_s[2] = (imu.zacc * dt + _hil_prev_accel[2]) / 2.0f;
-		memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_integral_m_s[0], sizeof(_hil_prev_accel));
+		hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt;
+		hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt;
+		hil_sensors.accelerometer_integral_m_s[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]) * dt;
+		memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_m_s2[0], sizeof(_hil_prev_accel));
 		hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f;
 		hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?
 		hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16
@@ -1493,12 +1492,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 	/* battery status */
 	{
-		struct battery_status_s hil_battery_status;
-		memset(&hil_battery_status, 0, sizeof(hil_battery_status));
+		struct battery_status_s hil_battery_status = {};
 
 		hil_battery_status.timestamp = timestamp;
-		hil_battery_status.voltage_v = 11.1f;
-		hil_battery_status.voltage_filtered_v = 11.1f;
+		hil_battery_status.voltage_v = 11.5f;
+		hil_battery_status.voltage_filtered_v = 11.5f;
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;
 
@@ -1759,7 +1757,7 @@ MavlinkReceiver::receive_thread(void *arg)
 	uint8_t buf[1600];
 #else
 	/* the serial port buffers internally as well, we just need to fit a small chunk */
-	uint8_t buf[32];
+	uint8_t buf[64];
 #endif
 	mavlink_message_t msg;
 
@@ -1799,10 +1797,17 @@ MavlinkReceiver::receive_thread(void *arg)
 	while (!_mavlink->_task_should_exit) {
 		if (poll(&fds[0], 1, timeout) > 0) {
 			if (_mavlink->get_protocol() == SERIAL) {
+
+				/*
+				 * to avoid reading very small chunks wait for data before reading
+				 * this is designed to target one message, so >20 bytes at a time
+				 */
+				const unsigned character_count = 20;
+
 				/* non-blocking read. read may return negative values */
-				if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) {
-					/* to avoid reading very small chunks wait for data before reading */
-					usleep(1000);
+				if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)character_count) {
+					unsigned sleeptime = (1.0f / (_mavlink->get_baudrate() / 10)) * character_count * 1000000;
+					usleep(sleeptime);
 				}
 			}
 #ifdef __PX4_POSIX

src/modules/mc_att_control/CMakeLists.txt

@@ -40,7 +40,6 @@ px4_add_module(
 	COMPILE_FLAGS ${MODULE_CFLAGS}
 	SRCS
 		mc_att_control_main.cpp
-		mc_att_control_params.c
 	DEPENDS
 		platforms__common
 	)

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -39,7 +39,6 @@
  * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors.
  * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011.
  *
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lorenz@px4.io>
  * @author Anton Babushkin <anton.babushkin@me.com>
  *
@@ -190,12 +189,10 @@ private:
 		param_t pitch_rate_max;
 		param_t yaw_rate_max;
 
-		param_t man_roll_max;
-		param_t man_pitch_max;
-		param_t man_yaw_max;
 		param_t acro_roll_max;
 		param_t acro_pitch_max;
 		param_t acro_yaw_max;
+		param_t rattitude_thres;
 
 	}		_params_handles;		/**< handles for interesting parameters */
 
@@ -212,11 +209,8 @@ private:
 		float yaw_rate_max;
 		math::Vector<3> mc_rate_max;		/**< attitude rate limits in stabilized modes */
 
-		float man_roll_max;
-		float man_pitch_max;
-		float man_yaw_max;
 		math::Vector<3> acro_rate_max;		/**< max attitude rates in acro mode */
-
+		float rattitude_thres;
 	}		_params;
 
 	/**
@@ -346,11 +340,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.roll_rate_max = 0.0f;
 	_params.pitch_rate_max = 0.0f;
 	_params.yaw_rate_max = 0.0f;
-	_params.man_roll_max = 0.0f;
-	_params.man_pitch_max = 0.0f;
-	_params.man_yaw_max = 0.0f;
 	_params.mc_rate_max.zero();
 	_params.acro_rate_max.zero();
+	_params.rattitude_thres = 1.0f;
 
 	_rates_prev.zero();
 	_rates_sp.zero();
@@ -380,12 +372,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.roll_rate_max	= 	param_find("MC_ROLLRATE_MAX");
 	_params_handles.pitch_rate_max	= 	param_find("MC_PITCHRATE_MAX");
 	_params_handles.yaw_rate_max	= 	param_find("MC_YAWRATE_MAX");
-	_params_handles.man_roll_max	= 	param_find("MC_MAN_R_MAX");
-	_params_handles.man_pitch_max	= 	param_find("MC_MAN_P_MAX");
-	_params_handles.man_yaw_max		= 	param_find("MC_MAN_Y_MAX");
 	_params_handles.acro_roll_max	= 	param_find("MC_ACRO_R_MAX");
 	_params_handles.acro_pitch_max	= 	param_find("MC_ACRO_P_MAX");
-	_params_handles.acro_yaw_max		= 	param_find("MC_ACRO_Y_MAX");
+	_params_handles.acro_yaw_max	= 	param_find("MC_ACRO_Y_MAX");
+	_params_handles.rattitude_thres = 	param_find("MC_RATT_TH");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -466,14 +456,6 @@ MulticopterAttitudeControl::parameters_update()
 	param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
 	_params.mc_rate_max(2) = math::radians(_params.yaw_rate_max);
 
-	/* manual attitude control scale */
-	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);
-	_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);
-
 	/* manual rate control scale and auto mode roll/pitch rate limits */
 	param_get(_params_handles.acro_roll_max, &v);
 	_params.acro_rate_max(0) = math::radians(v);
@@ -482,6 +464,9 @@ MulticopterAttitudeControl::parameters_update()
 	param_get(_params_handles.acro_yaw_max, &v);
 	_params.acro_rate_max(2) = math::radians(v);
 
+	/* stick deflection needed in rattitude mode to control rates not angles */
+	param_get(_params_handles.rattitude_thres, &_params.rattitude_thres);
+
 	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
 
 	return OK;
@@ -809,23 +794,34 @@ MulticopterAttitudeControl::task_main()
 			vehicle_status_poll();
 			vehicle_motor_limits_poll();
 
+			/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
+			 * or roll (yaw can rotate 360 in normal att control).  If both are true don't 
+			 * even bother running the attitude controllers */
+			if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){
+				if (fabsf(_manual_control_sp.y) > _params.rattitude_thres ||
+				    fabsf(_manual_control_sp.x) > _params.rattitude_thres){
+					 	_v_control_mode.flag_control_attitude_enabled = false;
+					 }
+			}
+
 			if (_v_control_mode.flag_control_attitude_enabled) {
+
 				control_attitude(dt);
 
-				/* publish attitude rates setpoint */
-				_v_rates_sp.roll = _rates_sp(0);
-				_v_rates_sp.pitch = _rates_sp(1);
-				_v_rates_sp.yaw = _rates_sp(2);
-				_v_rates_sp.thrust = _thrust_sp;
-				_v_rates_sp.timestamp = hrt_absolute_time();
+					/* publish attitude rates setpoint */
+					_v_rates_sp.roll = _rates_sp(0);
+					_v_rates_sp.pitch = _rates_sp(1);
+					_v_rates_sp.yaw = _rates_sp(2);
+					_v_rates_sp.thrust = _thrust_sp;
+					_v_rates_sp.timestamp = hrt_absolute_time();
 
-				if (_v_rates_sp_pub != nullptr) {
-					orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
-
-				} else if (_rates_sp_id) {
-					_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
-				}
+					if (_v_rates_sp_pub != nullptr) {
+						orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
 
+					} else if (_rates_sp_id) {
+						_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
+					}
+				//}
 			} else {
 				/* attitude controller disabled, poll rates setpoint topic */
 				if (_v_control_mode.flag_control_manual_enabled) {
@@ -976,4 +972,4 @@ int mc_att_control_main(int argc, char *argv[])
 
 	warnx("unrecognized command");
 	return 1;
-}
+}

src/modules/mc_att_control/mc_att_control_params.c

@@ -35,13 +35,10 @@
  * @file mc_att_control_params.c
  * Parameters for multicopter attitude controller.
  *
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lorenz@px4.io>
  * @author Anton Babushkin <anton@px4.io>
  */
 
-#include <systemlib/param/param.h>
-
 /**
  * Roll P gain
  *
@@ -251,7 +248,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 60.0f);
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
+PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 360.0f);
 
 /**
  * Max acro pitch rate
@@ -261,7 +258,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
  * @max 360.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
+PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 360.0f);
 
 /**
  * Max acro yaw rate
@@ -270,4 +267,17 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
  * @min 0.0
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f);
+PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f);
+
+/**
+ * Threshold for Rattitude mode
+ * 
+ * Manual input needed in order to override attitude control rate setpoints
+ * and instead pass manual stick inputs as rate setpoints
+ * 
+ * @unit 
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Attitude Control
+ */
+ PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f);

src/modules/mc_pos_control/CMakeLists.txt

@@ -36,7 +36,6 @@ px4_add_module(
 	STACK 1200
 	SRCS
 		mc_pos_control_main.cpp
-		mc_pos_control_params.c
 	DEPENDS
 		platforms__common
 	)

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -716,20 +716,22 @@ MulticopterPositionControl::control_manual(float dt)
 		/* check for pos. hold */
 		if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz)
 		{
-			if (!_pos_hold_engaged && (_params.hold_max_xy < FLT_EPSILON ||
-																 (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)))
-			{
-				_pos_hold_engaged = true;
+			if (!_pos_hold_engaged) {
+				if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)) {
+					_pos_hold_engaged = true;
+				} else {
+					_pos_hold_engaged = false;
+				}
 			}
 		}
 		else {
 			_pos_hold_engaged = false;
-			_pos_sp(0) = _pos(0);
-			_pos_sp(1) = _pos(1);
 		}
 
 		/* set requested velocity setpoint */
 		if (!_pos_hold_engaged) {
+			_pos_sp(0) = _pos(0);
+			_pos_sp(1) = _pos(1);
 			_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
 			_vel_sp(0) = req_vel_sp_scaled(0);
 			_vel_sp(1) = req_vel_sp_scaled(1);
@@ -859,18 +861,45 @@ void MulticopterPositionControl::control_auto(float dt)
 		}
 	}
 
+	bool current_setpoint_valid = false;
+	bool previous_setpoint_valid = false;
+
+	math::Vector<3> prev_sp;
+	math::Vector<3> curr_sp;
+
 	if (_pos_sp_triplet.current.valid) {
-		/* in case of interrupted mission don't go to waypoint but stay at current position */
-		_reset_pos_sp = true;
-		_reset_alt_sp = true;
 
 		/* project setpoint to local frame */
-		math::Vector<3> curr_sp;
 		map_projection_project(&_ref_pos,
 				       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
 				       &curr_sp.data[0], &curr_sp.data[1]);
 		curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
 
+		if (PX4_ISFINITE(curr_sp(0)) &&
+			PX4_ISFINITE(curr_sp(1)) &&
+			PX4_ISFINITE(curr_sp(2))) {
+			current_setpoint_valid = true;
+		}
+	}
+
+	if (_pos_sp_triplet.previous.valid) {
+		map_projection_project(&_ref_pos,
+					   _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
+					   &prev_sp.data[0], &prev_sp.data[1]);
+		prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
+
+		if (PX4_ISFINITE(prev_sp(0)) &&
+			PX4_ISFINITE(prev_sp(1)) &&
+			PX4_ISFINITE(prev_sp(2))) {
+			previous_setpoint_valid = true;
+		}
+	}
+
+	if (current_setpoint_valid) {
+		/* in case of interrupted mission don't go to waypoint but stay at current position */
+		_reset_pos_sp = true;
+		_reset_alt_sp = true;
+
 		/* scaled space: 1 == position error resulting max allowed speed */
 		math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max);	// TODO add mult param here
 
@@ -880,13 +909,8 @@ void MulticopterPositionControl::control_auto(float dt)
 		/* by default use current setpoint as is */
 		math::Vector<3> pos_sp_s = curr_sp_s;
 
-		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
+		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && previous_setpoint_valid) {
 			/* follow "previous - current" line */
-			math::Vector<3> prev_sp;
-			map_projection_project(&_ref_pos,
-						   _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
-						   &prev_sp.data[0], &prev_sp.data[1]);
-			prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
 
 			if ((curr_sp - prev_sp).length() > MIN_DIST) {
 

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -38,8 +38,6 @@
  * @author Anton Babushkin <anton@px4.io>
  */
 
-#include <systemlib/param/param.h>
-
 /**
  * Minimum thrust in auto thrust control
  *

src/modules/navigator/rtl.cpp

@@ -58,6 +58,7 @@
 RTL::RTL(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_rtl_state(RTL_STATE_NONE),
+	_rtl_start_lock(false),
 	_param_return_alt(this, "RTL_RETURN_ALT", false),
 	_param_descend_alt(this, "RTL_DESCEND_ALT", false),
 	_param_land_delay(this, "RTL_LAND_DELAY", false)
@@ -95,6 +96,7 @@ RTL::on_activation()
 		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
 			   + _param_return_alt.get()) {
 			_rtl_state = RTL_STATE_CLIMB;
+			_rtl_start_lock = false;
 
 		/* otherwise go straight to return */
 		} else {
@@ -102,6 +104,7 @@ RTL::on_activation()
 			_rtl_state = RTL_STATE_RETURN;
 			_mission_item.altitude_is_relative = false;
 			_mission_item.altitude = _navigator->get_global_position()->alt;
+			_rtl_start_lock = false;
 		}
 
 	}
@@ -126,7 +129,10 @@ RTL::set_rtl_item()
 	/* make sure we have the latest params */
 	updateParams();
 
-	set_previous_pos_setpoint();
+	if (!_rtl_start_lock) {
+		set_previous_pos_setpoint();
+	}
+
 	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
@@ -182,6 +188,8 @@ RTL::set_rtl_item()
 		mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)",
 			(int)(_mission_item.altitude),
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
+
+		_rtl_start_lock = true;
 		break;
 	}
 
@@ -190,7 +198,7 @@ RTL::set_rtl_item()
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		_mission_item.altitude_is_relative = false;
 		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
-		_mission_item.yaw = NAN;
+		_mission_item.yaw = _navigator->get_home_position()->yaw;
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
@@ -213,7 +221,7 @@ RTL::set_rtl_item()
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		_mission_item.altitude_is_relative = false;
 		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
-		_mission_item.yaw = NAN;
+		_mission_item.yaw = _navigator->get_home_position()->yaw;
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
@@ -239,7 +247,7 @@ RTL::set_rtl_item()
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		_mission_item.altitude_is_relative = false;
 		_mission_item.altitude = _navigator->get_home_position()->alt;
-		_mission_item.yaw = NAN;
+		_mission_item.yaw = _navigator->get_home_position()->yaw;
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LAND;
@@ -258,6 +266,7 @@ RTL::set_rtl_item()
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		_mission_item.altitude_is_relative = false;
 		_mission_item.altitude = _navigator->get_home_position()->alt;
+		// Do not change / control yaw in landed
 		_mission_item.yaw = NAN;
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;

src/modules/navigator/rtl.h

@@ -87,6 +87,8 @@ private:
 		RTL_STATE_LANDED,
 	} _rtl_state;
 
+	bool _rtl_start_lock;
+
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;

src/modules/position_estimator_inav/CMakeLists.txt

@@ -31,7 +31,7 @@
 #
 ############################################################################
 if(${OS} STREQUAL "nuttx")
-	list(APPEND MODULE_CFLAGS -Wframe-larger-than=3800)
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=3890)
 endif()
 px4_add_module(
 	MODULE modules__position_estimator_inav

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -38,7 +38,6 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  * @author Nuno Marques <n.marques21@hotmail.com>
  */
-
 #include <px4_posix.h>
 #include <unistd.h>
 #include <stdlib.h>
@@ -66,6 +65,7 @@
 #include <uORB/topics/att_pos_mocap.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/optical_flow.h>
+#include <uORB/topics/distance_sensor.h>
 #include <mavlink/mavlink_log.h>
 #include <poll.h>
 #include <systemlib/err.h>
@@ -90,8 +90,8 @@ static const hrt_abstime vision_topic_timeout = 500000;	// Vision topic timeout
 static const hrt_abstime mocap_topic_timeout = 500000;		// Mocap topic timeout = 0.5s
 static const hrt_abstime gps_topic_timeout = 500000;		// GPS topic timeout = 0.5s
 static const hrt_abstime flow_topic_timeout = 1000000;	// optical flow topic timeout = 1s
-static const hrt_abstime sonar_timeout = 150000;	// sonar timeout = 150ms
-static const hrt_abstime sonar_valid_timeout = 1000000;	// estimate sonar distance during this time after sonar loss
+static const hrt_abstime lidar_timeout = 150000;	// lidar timeout = 150ms
+static const hrt_abstime lidar_valid_timeout = 1000000;	// estimate lidar distance during this time after lidar loss
 static const unsigned updates_counter_len = 1000000;
 static const float max_flow = 1.0f;	// max flow value that can be used, rad/s
 
@@ -116,11 +116,11 @@ static inline int max(int val1, int val2)
  */
 static void usage(const char *reason)
 {
-	if (reason) {
-		PX4_INFO("%s\n", reason);
+	if (reason && *reason) {
+		PX4_INFO("%s", reason);
 	}
 
-	PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n\n");
+	PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n");
 	return;
 }
 
@@ -311,19 +311,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		{ 0.0f },		// D (pos)
 	};
 
-	float corr_sonar = 0.0f;
-	float corr_sonar_filtered = 0.0f;
+	float corr_lidar = 0.0f;
+	float corr_lidar_filtered = 0.0f;
 
 	float corr_flow[] = { 0.0f, 0.0f };	// N E
 	float w_flow = 0.0f;
 
-	float sonar_prev = 0.0f;
+	float lidar_prev = 0.0f;
 	//hrt_abstime flow_prev = 0;			// time of last flow measurement
-	hrt_abstime sonar_time = 0;			// time of last sonar measurement (not filtered)
-	hrt_abstime sonar_valid_time = 0;	// time of last sonar measurement used for correction (filtered)
+	hrt_abstime lidar_time = 0;			// time of last lidar measurement (not filtered)
+	hrt_abstime lidar_valid_time = 0;	// time of last lidar measurement used for correction (filtered)
+
+	int n_flow = 0;
+	float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f };
+	float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f };
+	float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };
+	float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };
+	float yaw_comp[] = { 0.0f, 0.0f };
 
 	bool gps_valid = false;			// GPS is valid
-	bool sonar_valid = false;		// sonar is valid
+	bool lidar_valid = false;		// lidar is valid
 	bool flow_valid = false;		// flow is valid
 	bool flow_accurate = false;		// flow should be accurate (this flag not updated if flow_valid == false)
 	bool vision_valid = false;		// vision is valid
@@ -352,6 +359,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	memset(&mocap, 0, sizeof(mocap));
 	struct vehicle_global_position_s global_pos;
 	memset(&global_pos, 0, sizeof(global_pos));
+	struct distance_sensor_s lidar;
+	memset(&lidar, 0, sizeof(lidar));
 
 	/* subscribe */
 	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -364,6 +373,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
 	int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
 	int home_position_sub = orb_subscribe(ORB_ID(home_position));
+	int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));
 
 	/* advertise */
 	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
@@ -445,6 +455,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			attitude_updates++;
 
 			bool updated;
+			bool updated2;
 
 			/* parameter update */
 			orb_check(parameter_update_sub, &updated);
@@ -510,56 +521,60 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			/* optical flow */
 			orb_check(optical_flow_sub, &updated);
+			orb_check(distance_sensor_sub, &updated2);
 
-			if (updated) {
+			if (updated && updated2) {
 				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
+				orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
 
 				/* calculate time from previous update */
 //				float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
 //				flow_prev = flow.flow_timestamp;
 
-				if ((flow.ground_distance_m > 0.31f) &&
-					(flow.ground_distance_m < 4.0f) &&
-					(PX4_R(att.R, 2, 2) > 0.7f) &&
-					(fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) {
+				if ((lidar.current_distance > 0.21f) &&
+					(lidar.current_distance < 4.0f) &&
+					/*(PX4_R(att.R, 2, 2) > 0.7f) &&*/
+					(fabsf(lidar.current_distance - lidar_prev) > FLT_EPSILON)) {
 
-					sonar_time = t;
-					sonar_prev = flow.ground_distance_m;
-					corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
-					corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt;
+					lidar_time = t;
+					lidar_prev = lidar.current_distance;
+					corr_lidar = lidar.current_distance + surface_offset + z_est[0];
+					corr_lidar_filtered += (corr_lidar - corr_lidar_filtered) * params.lidar_filt;
 
-					if (fabsf(corr_sonar) > params.sonar_err) {
+					if (fabsf(corr_lidar) > params.lidar_err) {
 						/* correction is too large: spike or new ground level? */
-						if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) {
+						if (fabsf(corr_lidar - corr_lidar_filtered) > params.lidar_err) {
 							/* spike detected, ignore */
-							corr_sonar = 0.0f;
-							sonar_valid = false;
+							corr_lidar = 0.0f;
+							lidar_valid = false;
 
 						} else {
 							/* new ground level */
-							surface_offset -= corr_sonar;
+							surface_offset -= corr_lidar;
 							surface_offset_rate = 0.0f;
-							corr_sonar = 0.0f;
-							corr_sonar_filtered = 0.0f;
-							sonar_valid_time = t;
-							sonar_valid = true;
+							corr_lidar = 0.0f;
+							corr_lidar_filtered = 0.0f;
+							lidar_valid_time = t;
+							lidar_valid = true;
 							local_pos.surface_bottom_timestamp = t;
 							mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset);
 						}
 
 					} else {
 						/* correction is ok, use it */
-						sonar_valid_time = t;
-						sonar_valid = true;
+						lidar_valid_time = t;
+						lidar_valid = true;
 					}
 				}
 
 				float flow_q = flow.quality / 255.0f;
-				float dist_bottom = - z_est[0] - surface_offset;
+				float dist_bottom = - z_est[0] - surface_offset; //lidar.current_distance;
 
-				if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) {
+				if (dist_bottom > 0.21f && flow_q > params.flow_q_min) {
 					/* distance to surface */
-					float flow_dist = dist_bottom / PX4_R(att.R, 2, 2);
+					//float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar
+					float flow_dist = dist_bottom; //use this if using lidar
+
 					/* check if flow if too large for accurate measurements */
 					/* calculate estimated velocity in body frame */
 					float body_v_est[2] = { 0.0f, 0.0f };
@@ -571,12 +586,48 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
 					flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
 							fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
+					//this flag is not working -->
+					flow_accurate = true; //already checked if flow_q > 0.3
+
+
+					/*calculate offset of flow-gyro using already calibrated gyro from autopilot*/
+					flow_gyrospeed[0] = flow.gyro_x_rate_integral/(float)flow.integration_timespan*1000000.0f;
+					flow_gyrospeed[1] = flow.gyro_y_rate_integral/(float)flow.integration_timespan*1000000.0f;
+					flow_gyrospeed[2] = flow.gyro_z_rate_integral/(float)flow.integration_timespan*1000000.0f;
+					
+					//moving average
+					if (n_flow >= 100) {
+						gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0];
+						gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1];
+						gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2];
+						n_flow = 0;
+						flow_gyrospeed_filtered[0] = 0.0f;
+						flow_gyrospeed_filtered[1] = 0.0f;
+						flow_gyrospeed_filtered[2] = 0.0f;
+						att_gyrospeed_filtered[0] = 0.0f;
+						att_gyrospeed_filtered[1] = 0.0f;
+						att_gyrospeed_filtered[2] = 0.0f;
+					} else {
+						flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1);
+						flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1);
+						flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1);
+						att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1);
+						att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1);
+						att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1);
+						n_flow++;
+					}
+					
+
+					/*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/
+					yaw_comp[0] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
+					yaw_comp[1] =  - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
+
 
 					/* convert raw flow to angular flow (rad/s) */
 					float flow_ang[2];
-					//todo check direction of x und y axis
-					flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
-					flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
+					//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
+					flow_ang[0] = (flow.pixel_flow_x_integral - flow.gyro_x_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[0] - yaw_comp[0];//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
+					flow_ang[1] = (flow.pixel_flow_y_integral - flow.gyro_y_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[1] - yaw_comp[1];//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
 					/* flow measurements vector */
 					float flow_m[3];
 					flow_m[0] = -flow_ang[0] * flow_dist;
@@ -599,6 +650,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
 					w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
 
+
 					/* if flow is not accurate, reduce weight for it */
 					// TODO make this more fuzzy
 					if (!flow_accurate) {
@@ -862,9 +914,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		}
 
 		/* check for timeout on FLOW topic */
-		if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) {
+		if ((flow_valid || lidar_valid) && t > flow.timestamp + flow_topic_timeout) {
 			flow_valid = false;
-			sonar_valid = false;
+			lidar_valid = false;
 			warnx("FLOW timeout");
 			mavlink_log_info(mavlink_fd, "[inav] FLOW timeout");
 		}
@@ -890,10 +942,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout");
 		}
 
-		/* check for sonar measurement timeout */
-		if (sonar_valid && (t > (sonar_time + sonar_timeout))) {
-			corr_sonar = 0.0f;
-			sonar_valid = false;
+		/* check for lidar measurement timeout */
+		if (lidar_valid && (t > (lidar_time + lidar_timeout))) {
+			corr_lidar = 0.0f;
+			lidar_valid = false;
 		}
 
 		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
@@ -921,16 +973,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;
 
-		bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
+		bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout);
 
 		if (dist_bottom_valid) {
 			/* surface distance prediction */
 			surface_offset += surface_offset_rate * dt;
 
 			/* surface distance correction */
-			if (sonar_valid) {
-				surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt;
-				surface_offset -= corr_sonar * params.w_z_sonar * dt;
+			if (lidar_valid) {
+				surface_offset_rate -= corr_lidar * 0.5f * params.w_z_lidar * params.w_z_lidar * dt;
+				surface_offset -= corr_lidar * params.w_z_lidar * dt;
 			}
 		}
 
@@ -1114,6 +1166,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
 				inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
+				//mavlink_log_info(mavlink_fd, "w_flow = %2.4f\t w_xy_flow = %2.4f\n", (double)w_flow, (double)params.w_xy_flow);
 			}
 
 			if (use_gps_xy) {
@@ -1229,7 +1282,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			if (local_pos.dist_bottom_valid) {
 				local_pos.dist_bottom = -z_est[0] - surface_offset;
-				local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate;
+				local_pos.dist_bottom_rate = - z_est[1] - surface_offset_rate;
 			}
 
 			local_pos.timestamp = t;

src/modules/position_estimator_inav/position_estimator_inav_params.c

@@ -86,15 +86,15 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f);
 PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 5.0f);
 
 /**
- * Z axis weight for sonar
+ * Z axis weight for lidar
  *
- * Weight (cutoff frequency) for sonar measurements.
+ * Weight (cutoff frequency) for lidar measurements.
  *
  * @min 0.0
  * @max 10.0
  * @group Position Estimator INAV
  */
-PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
+PARAM_DEFINE_FLOAT(INAV_W_Z_LIDAR, 3.0f);
 
 /**
  * XY axis weight for GPS position
@@ -158,10 +158,10 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f);
  * Weight (cutoff frequency) for optical flow (velocity) measurements.
  *
  * @min 0.0
- * @max 10.0
+ * @max 30.0
  * @group Position Estimator INAV
  */
-PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 10.0f);
 
 /**
  * XY axis weight for resetting velocity
@@ -217,18 +217,18 @@ PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
  * @max 1.0
  * @group Position Estimator INAV
  */
-PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.3f);
 
 /**
- * Weight for sonar filter
+ * Weight for lidar filter
  *
- * Sonar filter detects spikes on sonar measurements and used to detect new surface level.
+ * Lidar filter detects spikes on lidar measurements and used to detect new surface level.
  *
  * @min 0.0
  * @max 1.0
  * @group Position Estimator INAV
  */
-PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
+PARAM_DEFINE_FLOAT(INAV_LIDAR_FILT, 0.05f);
 
 /**
  * Sonar maximal error for new surface
@@ -240,7 +240,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
  * @unit m
  * @group Position Estimator INAV
  */
-PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_LIDAR_ERR, 0.5f);
 
 /**
  * Land detector time
@@ -289,6 +289,30 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
  */
 PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
 
+/**
+ * Flow module offset (center of rotation) in X direction
+ *
+ * Yaw X flow compensation
+ *
+ * @min -1.0
+ * @max 1.0
+ * @unit m
+ * @group Position Estimator INAV
+ */
+PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f);
+
+/**
+ * Flow module offset (center of rotation) in Y direction
+ *
+ * Yaw Y flow compensation
+ *
+ * @min -1.0
+ * @max 1.0
+ * @unit m
+ * @group Position Estimator INAV
+ */
+PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f);
+
 /**
  * Disable vision input
  *
@@ -319,7 +343,7 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h)
 	h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
 	h->w_z_gps_v = param_find("INAV_W_Z_GPS_V");
 	h->w_z_vision_p = param_find("INAV_W_Z_VIS_P");
-	h->w_z_sonar = param_find("INAV_W_Z_SONAR");
+	h->w_z_lidar = param_find("INAV_W_Z_LIDAR");
 	h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
 	h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
 	h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P");
@@ -331,13 +355,15 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h)
 	h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
 	h->flow_k = param_find("INAV_FLOW_K");
 	h->flow_q_min = param_find("INAV_FLOW_Q_MIN");
-	h->sonar_filt = param_find("INAV_SONAR_FILT");
-	h->sonar_err = param_find("INAV_SONAR_ERR");
+	h->lidar_filt = param_find("INAV_LIDAR_FILT");
+	h->lidar_err = param_find("INAV_LIDAR_ERR");
 	h->land_t = param_find("INAV_LAND_T");
 	h->land_disp = param_find("INAV_LAND_DISP");
 	h->land_thr = param_find("INAV_LAND_THR");
 	h->no_vision = param_find("CBRK_NO_VISION");
 	h->delay_gps = param_find("INAV_DELAY_GPS");
+	h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X");
+	h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y");
 
 	return 0;
 }
@@ -347,7 +373,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h
 	param_get(h->w_z_baro, &(p->w_z_baro));
 	param_get(h->w_z_gps_p, &(p->w_z_gps_p));
 	param_get(h->w_z_vision_p, &(p->w_z_vision_p));
-	param_get(h->w_z_sonar, &(p->w_z_sonar));
+	param_get(h->w_z_lidar, &(p->w_z_lidar));
 	param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
 	param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
 	param_get(h->w_xy_vision_p, &(p->w_xy_vision_p));
@@ -359,13 +385,15 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h
 	param_get(h->w_acc_bias, &(p->w_acc_bias));
 	param_get(h->flow_k, &(p->flow_k));
 	param_get(h->flow_q_min, &(p->flow_q_min));
-	param_get(h->sonar_filt, &(p->sonar_filt));
-	param_get(h->sonar_err, &(p->sonar_err));
+	param_get(h->lidar_filt, &(p->lidar_filt));
+	param_get(h->lidar_err, &(p->lidar_err));
 	param_get(h->land_t, &(p->land_t));
 	param_get(h->land_disp, &(p->land_disp));
 	param_get(h->land_thr, &(p->land_thr));
 	param_get(h->no_vision, &(p->no_vision));
 	param_get(h->delay_gps, &(p->delay_gps));
+	param_get(h->flow_module_offset_x, &(p->flow_module_offset_x));
+	param_get(h->flow_module_offset_y, &(p->flow_module_offset_y));
 
 	return 0;
 }

src/modules/position_estimator_inav/position_estimator_inav_params.h

@@ -46,7 +46,7 @@ struct position_estimator_inav_params {
 	float w_z_gps_p;
 	float w_z_gps_v;
 	float w_z_vision_p;
-	float w_z_sonar;
+	float w_z_lidar;
 	float w_xy_gps_p;
 	float w_xy_gps_v;
 	float w_xy_vision_p;
@@ -58,13 +58,15 @@ struct position_estimator_inav_params {
 	float w_acc_bias;
 	float flow_k;
 	float flow_q_min;
-	float sonar_filt;
-	float sonar_err;
+	float lidar_filt;
+	float lidar_err;
 	float land_t;
 	float land_disp;
 	float land_thr;
 	int32_t no_vision;
 	float delay_gps;
+	float flow_module_offset_x;
+	float flow_module_offset_y;
 };
 
 struct position_estimator_inav_param_handles {
@@ -72,7 +74,7 @@ struct position_estimator_inav_param_handles {
 	param_t w_z_gps_p;
 	param_t w_z_gps_v;
 	param_t w_z_vision_p;
-	param_t w_z_sonar;
+	param_t w_z_lidar;
 	param_t w_xy_gps_p;
 	param_t w_xy_gps_v;
 	param_t w_xy_vision_p;
@@ -84,13 +86,15 @@ struct position_estimator_inav_param_handles {
 	param_t w_acc_bias;
 	param_t flow_k;
 	param_t flow_q_min;
-	param_t sonar_filt;
-	param_t sonar_err;
+	param_t lidar_filt;
+	param_t lidar_err;
 	param_t land_t;
 	param_t land_disp;
 	param_t land_thr;
 	param_t no_vision;
 	param_t delay_gps;
+	param_t flow_module_offset_x;
+	param_t flow_module_offset_y;
 };
 
 #define CBRK_NO_VISION_KEY	328754

src/modules/px4iofirmware/mixer.cpp

@@ -88,9 +88,6 @@ static int	mixer_callback(uintptr_t handle,
 
 static MixerGroup mixer_group(mixer_callback, 0);
 
-/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
-static void mixer_set_failsafe();
-
 void
 mixer_tick(void)
 {
@@ -479,15 +476,6 @@ mixer_handle_text(const void *buffer, size_t length)
 		/* if anything was parsed */
 		if (resid != mixer_text_length) {
 
-			/* only set mixer ok if no residual is left over */
-			if (resid == 0) {
-				r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
-
-			} else {
-				/* not yet reached the end of the mixer, set as not ok */
-				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
-			}
-
 			isr_debug(2, "used %u", mixer_text_length - resid);
 
 			/* copy any leftover text to the base of the buffer for re-use */
@@ -496,9 +484,6 @@ mixer_handle_text(const void *buffer, size_t length)
 			}
 
 			mixer_text_length = resid;
-
-			/* update failsafe values */
-			mixer_set_failsafe();
 		}
 
 		break;
@@ -507,7 +492,7 @@ mixer_handle_text(const void *buffer, size_t length)
 	return 0;
 }
 
-static void
+void
 mixer_set_failsafe()
 {
 	/*

src/modules/px4iofirmware/px4io.h

@@ -190,6 +190,8 @@ extern pwm_limit_t pwm_limit;
  */
 extern void	mixer_tick(void);
 extern int	mixer_handle_text(const void *buffer, size_t length);
+/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
+extern void	mixer_set_failsafe(void);
 
 /**
  * Safety switch/LED.

src/modules/px4iofirmware/registers.c

@@ -469,8 +469,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * Allow FMU override of arming state (to allow in-air restores),
 			 * but only if the arming state is not in sync on the IO side.
 			 */
-			if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+
+			if (PX4IO_P_STATUS_FLAGS_MIXER_OK & value) {
+				r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
+
+			} else if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
 				r_status_flags = value;
+
+			}
+
+			if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) {
+
+				/* update failsafe values, now that the mixer is set to ok */
+				mixer_set_failsafe();
 			}
 
 			break;

src/modules/sdlog2/CMakeLists.txt

@@ -44,7 +44,6 @@ px4_add_module(
 	SRCS
 		sdlog2.c
 		logbuffer.c
-		params.c
 	DEPENDS
 		platforms__common
 	)

src/modules/sdlog2/params.c

@@ -31,8 +31,6 @@
  *
  ****************************************************************************/
 
-#include <systemlib/param/param.h>
-
 /**
  * Logging rate.
  *
@@ -43,7 +41,7 @@
  * commonly is before arming).
  *
  * @min -1
- * @max  1
+ * @max  100
  * @group SD Logging
  */
 PARAM_DEFINE_INT32(SDLOG_RATE, -1);

src/modules/sdlog2/sdlog2.c

@@ -1916,7 +1916,7 @@ void sdlog2_status()
 }
 
 /**
- * @return 0 if file exists
+ * @return true if file exists
  */
 bool file_exist(const char *filename)
 {

src/modules/sensors/sensor_params.c

@@ -1969,6 +1969,15 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
  */
 PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
 
+/**
+ * Rattitude switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Switches
+ */
+PARAM_DEFINE_INT32(RC_MAP_RATT_SW, 0);
+
 /**
  * Posctl switch channel mapping.
  *
@@ -2132,6 +2141,23 @@ PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f);
  */
 PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f);
 
+/**
+ * Threshold for selecting rattitude mode
+ *
+ * 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_RATT_TH, 0.5f);
+
 /**
  * Threshold for selecting posctl mode
  *

src/modules/sensors/sensors.cpp

@@ -269,6 +269,7 @@ private:
 
 		int rc_map_mode_sw;
 		int rc_map_return_sw;
+		int rc_map_rattitude_sw;
 		int rc_map_posctl_sw;
 		int rc_map_loiter_sw;
 		int rc_map_acro_sw;
@@ -287,6 +288,7 @@ private:
 		int32_t rc_fails_thr;
 		float rc_assist_th;
 		float rc_auto_th;
+		float rc_rattitude_th;
 		float rc_posctl_th;
 		float rc_return_th;
 		float rc_loiter_th;
@@ -294,6 +296,7 @@ private:
 		float rc_offboard_th;
 		bool rc_assist_inv;
 		bool rc_auto_inv;
+		bool rc_rattitude_inv;
 		bool rc_posctl_inv;
 		bool rc_return_inv;
 		bool rc_loiter_inv;
@@ -325,6 +328,7 @@ private:
 
 		param_t rc_map_mode_sw;
 		param_t rc_map_return_sw;
+		param_t rc_map_rattitude_sw;
 		param_t rc_map_posctl_sw;
 		param_t rc_map_loiter_sw;
 		param_t rc_map_acro_sw;
@@ -347,6 +351,7 @@ private:
 		param_t rc_fails_thr;
 		param_t rc_assist_th;
 		param_t rc_auto_th;
+		param_t rc_rattitude_th;
 		param_t rc_posctl_th;
 		param_t rc_return_th;
 		param_t rc_loiter_th;
@@ -575,6 +580,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
 
 	/* optional mode switches, not mapped per default */
+	_parameter_handles.rc_map_rattitude_sw = param_find("RC_MAP_RATT_SW");
 	_parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW");
 	_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
 	_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
@@ -598,6 +604,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR");
 	_parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH");
 	_parameter_handles.rc_auto_th = param_find("RC_AUTO_TH");
+	_parameter_handles.rc_rattitude_th = param_find("RC_RATT_TH");
 	_parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH");
 	_parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
 	_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
@@ -742,6 +749,10 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 	}
 
+	if (param_get(_parameter_handles.rc_map_rattitude_sw, &(_parameters.rc_map_rattitude_sw)) != OK) {
+		warnx("%s", paramerr);
+	}
+
 	if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) {
 		warnx("%s", paramerr);
 	}
@@ -779,6 +790,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th));
 	_parameters.rc_auto_inv = (_parameters.rc_auto_th < 0);
 	_parameters.rc_auto_th = fabs(_parameters.rc_auto_th);
+	param_get(_parameter_handles.rc_rattitude_th, &(_parameters.rc_rattitude_th));
+	_parameters.rc_rattitude_inv = (_parameters.rc_rattitude_th < 0);
+	_parameters.rc_rattitude_th = fabs(_parameters.rc_rattitude_th);
 	param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th));
 	_parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0);
 	_parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th);
@@ -803,6 +817,7 @@ Sensors::parameters_update()
 
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1;
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1;
+	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE] = _parameters.rc_map_rattitude_sw - 1;
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1;
 	_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;
@@ -1928,6 +1943,9 @@ Sensors::rc_poll()
 			/* mode switches */
 			manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th,
 					     _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
+			manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE,
+						  _parameters.rc_rattitude_th,
+						  _parameters.rc_rattitude_inv);
 			manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th,
 					       _parameters.rc_posctl_inv);
 			manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th,

src/modules/systemlib/mixer/mixer.h

@@ -481,7 +481,7 @@ public:
 	 *				compared to thrust.
 	 * @param yaw_wcale		Scaling factor applied to yaw inputs compared
 	 *				to thrust.
-	 * @param deadband		Minumum rotor control output value; usually
+	 * @param idle_speed		Minumum rotor control output value; usually
 	 *				tuned to ensure that rotors never stall at the
 	 * 				low end of their control range.
 	 */
@@ -491,7 +491,7 @@ public:
 			float roll_scale,
 			float pitch_scale,
 			float yaw_scale,
-			float deadband);
+			float idle_speed);
 	~MultirotorMixer();
 
 	/**

src/modules/systemlib/mixer/multi_tables.mk

@@ -1,42 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2014 Anton Matosov <anton.matosov@gmail.com>. 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.
-#
-############################################################################
-
-
-SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
-MULTI_TABLES := $(SELF_DIR)multi_tables.py
-
-# Add explicit dependency, as implicit one doesn't work often.
-$(SELF_DIR)mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h
-
-$(SELF_DIR)mixer_multirotor.generated.h : $(MULTI_TABLES)
-	$(Q) $(PYTHON) $(MULTI_TABLES) > $(SELF_DIR)mixer_multirotor.generated.h

src/modules/uavcan/uavcan_params.c

@@ -45,9 +45,10 @@
  *  0 - UAVCAN disabled.
  *  1 - Enabled support for UAVCAN actuators and sensors.
  *  2 - Enabled support for dynamic node ID allocation and firmware update.
+ *  3 - Sets the motor control outputs to UAVCAN and enables support for dynamic node ID allocation and firmware update.
  *
  * @min 0
- * @max 2
+ * @max 3
  * @group UAVCAN
  */
 PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0);

src/platforms/posix/drivers/accelsim/accelsim.cpp

@@ -41,15 +41,10 @@
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <stdint.h>
-#include <stdbool.h>
 #include <stddef.h>
 #include <stdlib.h>
-#include <semaphore.h>
 #include <string.h>
-#include <fcntl.h>
 #include <poll.h>
-#include <errno.h>
-#include <stdio.h>
 #include <math.h>
 #include <unistd.h>
 #include <px4_getopt.h>
@@ -64,7 +59,6 @@
 #include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/device/ringbuffer.h>
-#include <drivers/drv_tone_alarm.h>
 
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -107,11 +101,6 @@ public:
 	virtual ssize_t		read(device::file_t *filp, char *buffer, size_t buflen);
 	virtual int		ioctl(device::file_t *filp, int cmd, unsigned long arg);
 
-	/**
-	 * Diagnostics - print some basic information about the driver.
-	 */
-	//void			print_info();
-
 	/**
 	 * dump register values
 	 */
@@ -120,8 +109,8 @@ public:
 protected:
 	friend class 		ACCELSIM_mag;
 
-	virtual ssize_t		mag_read(device::file_t *filp, char *buffer, size_t buflen);
-	virtual int		mag_ioctl(device::file_t *filp, int cmd, unsigned long arg);
+	ssize_t			mag_read(device::file_t *filp, char *buffer, size_t buflen);
+	int			mag_ioctl(device::file_t *filp, int cmd, unsigned long arg);
 
 	int			transfer(uint8_t *send, uint8_t *recv, unsigned len);
 private:
@@ -177,10 +166,6 @@ private:
 	// this is used to support runtime checking of key
 	// configuration registers to detect SPI bus errors and sensor
 	// reset
-#define ACCELSIM_NUM_CHECKED_REGISTERS 1
-	static const uint8_t	_checked_registers[ACCELSIM_NUM_CHECKED_REGISTERS];
-	uint8_t			_checked_values[ACCELSIM_NUM_CHECKED_REGISTERS];
-	uint8_t			_checked_next;
 
 	/**
 	 * Start automatic measurement.
@@ -227,41 +212,6 @@ private:
 	 */
 	void			mag_measure();
 
-	/**
-	 * Read a register from the ACCELSIM
-	 *
-	 * @param		The register to read.
-	 * @return		The value that was read.
-	 */
-	uint8_t			read_reg(unsigned reg);
-
-	/**
-	 * Write a register in the ACCELSIM
-	 *
-	 * @param reg		The register to write.
-	 * @param value		The new value to write.
-	 */
-	void			write_reg(unsigned reg, uint8_t value);
-
-	/**
-	 * Modify a register in the ACCELSIM
-	 *
-	 * Bits are cleared before bits are set.
-	 *
-	 * @param reg		The register to modify.
-	 * @param clearbits	Bits in the register to clear.
-	 * @param setbits	Bits in the register to set.
-	 */
-	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
-
-	/**
-	 * Write a register in the ACCELSIM, updating _checked_values
-	 *
-	 * @param reg		The register to write.
-	 * @param value		The new value to write.
-	 */
-	void			write_checked_reg(unsigned reg, uint8_t value);
-
 	/**
 	 * Set the ACCELSIM accel measurement range.
 	 *
@@ -314,12 +264,6 @@ private:
 	ACCELSIM operator=(const ACCELSIM &);
 };
 
-/*
-  list of registers that will be checked in check_registers(). Note
-  that ADDR_WHO_AM_I must be first in the list.
- */
-const uint8_t ACCELSIM::_checked_registers[ACCELSIM_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I };
-
 /**
  * Helper class implementing the mag driver node.
  */
@@ -329,25 +273,24 @@ public:
 	ACCELSIM_mag(ACCELSIM *parent);
 	~ACCELSIM_mag();
 
-	virtual ssize_t			read(device::file_t *filp, char *buffer, size_t buflen);
-	virtual int			ioctl(device::file_t *filp, int cmd, unsigned long arg);
+	virtual ssize_t	read(device::file_t *filp, char *buffer, size_t buflen);
+	virtual int	ioctl(device::file_t *filp, int cmd, unsigned long arg);
 
-	virtual int			init();
+	virtual int	init();
 
 protected:
 	friend class ACCELSIM;
 
-	void				parent_poll_notify();
 private:
-	ACCELSIM				*_parent;
+	ACCELSIM	*_parent;
 
-	orb_advert_t			_mag_topic;
-	int				_mag_orb_class_instance;
-	int				_mag_class_instance;
+	orb_advert_t	_mag_topic;
+	int		_mag_orb_class_instance;
+	int		_mag_class_instance;
 
-	void				measure();
+	void		measure();
 
-	void				measure_trampoline(void *arg);
+	void		measure_trampoline(void *arg);
 
 	/* this class does not allow copying due to ptr data members */
 	ACCELSIM_mag(const ACCELSIM_mag &);
@@ -388,11 +331,8 @@ ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) :
 	_accel_filter_z(ACCELSIM_ACCEL_DEFAULT_RATE, ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_rotation(rotation),
 	_constant_accel_count(0),
-	_last_temperature(0),
-	_checked_next(0)
+	_last_temperature(0)
 {
-
-
 	// enable debug() calls
 	_debug_enabled = false;
 
@@ -886,53 +826,6 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg)
 	}
 }
 
-uint8_t
-ACCELSIM::read_reg(unsigned reg)
-{
-	uint8_t cmd[2];
-
-	cmd[0] = reg | DIR_READ;
-	cmd[1] = 0;
-
-	transfer(cmd, cmd, sizeof(cmd));
-
-	return cmd[1];
-}
-
-void
-ACCELSIM::write_reg(unsigned reg, uint8_t value)
-{
-	uint8_t	cmd[2];
-
-	cmd[0] = reg | DIR_WRITE;
-	cmd[1] = value;
-
-	transfer(cmd, nullptr, sizeof(cmd));
-}
-
-void
-ACCELSIM::write_checked_reg(unsigned reg, uint8_t value)
-{
-	write_reg(reg, value);
-
-	for (uint8_t i = 0; i < ACCELSIM_NUM_CHECKED_REGISTERS; i++) {
-		if (reg == _checked_registers[i]) {
-			_checked_values[i] = value;
-		}
-	}
-}
-
-void
-ACCELSIM::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
-{
-	uint8_t	val;
-
-	val = read_reg(reg);
-	val &= ~clearbits;
-	val |= setbits;
-	write_checked_reg(reg, val);
-}
-
 int
 ACCELSIM::accel_set_range(unsigned max_g)
 {
@@ -1086,51 +979,6 @@ ACCELSIM::measure()
 	accel_report.y_raw = (int16_t)(raw_accel_report.y / _accel_range_scale);
 	accel_report.z_raw = (int16_t)(raw_accel_report.z / _accel_range_scale);
 
-	// float xraw_f = (int16_t)(raw_accel_report.x/_accel_range_scale);
-	// float yraw_f = (int16_t)(raw_accel_report.y / _accel_range_scale);
-	// float zraw_f = (int16_t)(raw_accel_report.z / _accel_range_scale);
-
-	// // apply user specified rotation
-	// rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
-
-	// float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
-	// float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
-	// float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
-
-	/*
-	  we have logs where the accelerometers get stuck at a fixed
-	  large value. We want to detect this and mark the sensor as
-	  being faulty
-	 */
-	// if (fabsf(_last_accel[0] - x_in_new) < 0.001f &&
-	//     fabsf(_last_accel[1] - y_in_new) < 0.001f &&
-	//     fabsf(_last_accel[2] - z_in_new) < 0.001f &&
-	//     fabsf(x_in_new) > 20 &&
-	//     fabsf(y_in_new) > 20 &&
-	//     fabsf(z_in_new) > 20) {
-	// 	_constant_accel_count += 1;
-	// } else {
-	// 	_constant_accel_count = 0;
-	// }
-	// if (_constant_accel_count > 100) {
-	// 	// we've had 100 constant accel readings with large
-	// 	// values. The sensor is almost certainly dead. We
-	// 	// will raise the error_count so that the top level
-	// 	// flight code will know to avoid this sensor, but
-	// 	// we'll still give the data so that it can be logged
-	// 	// and viewed
-	// 	perf_count(_bad_values);
-	// 	_constant_accel_count = 0;
-	// }
-
-	// _last_accel[0] = x_in_new;
-	// _last_accel[1] = y_in_new;
-	// _last_accel[2] = z_in_new;
-
-	// accel_report.x = _accel_filter_x.apply(x_in_new);
-	// accel_report.y = _accel_filter_y.apply(y_in_new);
-	// accel_report.z = _accel_filter_z.apply(z_in_new);
-
 	accel_report.x = raw_accel_report.x;
 	accel_report.y = raw_accel_report.y;
 	accel_report.z = raw_accel_report.z;
@@ -1218,13 +1066,6 @@ ACCELSIM::mag_measure()
 	/* apply user specified rotation */
 	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
 
-	// mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
-	// mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
-	// mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
-	// mag_report.scaling = _mag_range_scale;
-	// mag_report.range_ga = (float)_mag_range_ga;
-	// mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
-
 	/* remember the temperature. The datasheet isn't clear, but it
 	 * seems to be a signed offset from 25 degrees C in units of 0.125C
 	 */
@@ -1283,12 +1124,6 @@ out:
 	return ret;
 }
 
-void
-ACCELSIM_mag::parent_poll_notify()
-{
-	poll_notify(POLLIN);
-}
-
 ssize_t
 ACCELSIM_mag::read(device::file_t *filp, char *buffer, size_t buflen)
 {
@@ -1414,7 +1249,6 @@ info()
 	}
 
 	PX4_DEBUG("state @ %p", g_dev);
-	//g_dev->print_info();
 
 	return 0;
 }

src/systemcmds/mixer/mixer.cpp

@@ -93,12 +93,12 @@ mixer_main(int argc, char *argv[])
 static void
 usage(const char *reason)
 {
-	if (reason) {
-		PX4_INFO("%s\n", reason);
+	if (reason && *reason) {
+		PX4_INFO("%s", reason);
 	}
 
-	PX4_INFO("usage:\n");
-	PX4_INFO("  mixer load <device> <filename>\n");
+	PX4_INFO("usage:");
+	PX4_INFO("  mixer load <device> <filename>");
 }
 
 static int