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