Merge branch 'master' of https://github.com/PX4/Firmware

2026-06-07 17:35:22 +08:00 · 2015-11-10 16:19:17 +01:00
parent 1235c1f0bd b3bb8adbc5
commit c1a67858a4
92 changed files with 1268 additions and 1394 deletions
@@ -25,3 +25,6 @@
 [submodule "unittests/googletest"]
 	path = unittests/googletest
 	url = https://github.com/google/googletest.git
+[submodule "src/lib/matrix"]
+	path = src/lib/matrix
+	url = https://github.com/PX4/Matrix.git
@@ -118,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
@@ -232,6 +232,7 @@ px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
 px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal")
 px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim")
 px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo")
+px4_add_git_submodule(TARGET git_matrix PATH "src/lib/matrix")

 add_custom_target(submodule_clean
 	WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
@@ -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>
 #

@@ -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
@@ -0,0 +1,33 @@
+#!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.05
+	param set MC_ROLLRATE_I 0.05
+	param set MC_ROLLRATE_D 0.002
+	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 980
+	param set MPC_THR_MIN 0.06
+	param set MPC_MANTHR_MIN 0.06
+fi
@@ -0,0 +1,146 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<parameters>
+  <version>3</version>
+  <group name="UAVCAN Motor Parameters" no_code_generation="true">
+    <parameter default="75" name="ctl_bw" type="INT32">
+      <short_desc>Speed controller bandwidth</short_desc>
+      <long_desc>Speed controller bandwidth, in Hz. Higher values result in faster speed and current rise times, but may result in overshoot and higher current consumption. For fixed-wing aircraft, this value should be less than 50 Hz; for multirotors, values up to 100 Hz may provide improvements in responsiveness.</long_desc>
+      <unit>Hertz</unit>
+      <min>10</min>
+      <max>250</max>
+    </parameter>
+    <parameter default="1" name="ctl_dir" type="INT32">
+      <short_desc>Reverse direction</short_desc>
+      <long_desc>Motor spin direction as detected during initial enumeration. Use 0 or 1 to reverse direction.</long_desc>
+      <min>0</min>
+      <max>1</max>
+    </parameter>
+        <parameter default="1" name="ctl_gain" type="FLOAT">
+      <short_desc>Speed (RPM) controller gain</short_desc>
+      <long_desc>Speed (RPM) controller gain. Determines controller
+            aggressiveness; units are amp-seconds per radian. Systems with
+            higher rotational inertia (large props) will need gain increased;
+            systems with low rotational inertia (small props) may need gain
+            decreased. Higher values result in faster response, but may result
+            in oscillation and excessive overshoot. Lower values result in a
+            slower, smoother response.</long_desc>
+    <unit>amp-seconds per radian</unit>
+    <decimal>3</decimal>
+      <min>0.00</min>
+      <max>1.00</max>
+    </parameter>
+      <parameter default="3.5" name="ctl_hz_idle" type="FLOAT">
+      <short_desc>Idle speed (e Hz)</short_desc>
+      <long_desc>Idle speed (e Hz)</long_desc>
+    <unit>Hertz</unit>
+    <decimal>3</decimal>
+      <min>0.0</min>
+      <max>100.0</max>
+    </parameter>
+      <parameter default="25" name="ctl_start_rate" type="INT32">
+      <short_desc>Spin-up rate (e Hz/s)</short_desc>
+      <long_desc>Spin-up rate (e Hz/s)</long_desc>
+    <unit>Hz/s</unit>
+    <decimal></decimal>
+      <min>5</min>
+      <max>1000</max>
+    </parameter>
+      <parameter default="0" name="esc_index" type="INT32">
+      <short_desc>Index of this ESC in throttle command messages.</short_desc>
+      <long_desc>Index of this ESC in throttle command messages.</long_desc>
+    <unit>Index</unit>
+    <decimal></decimal>
+      <min>0</min>
+      <max>15</max>
+    </parameter>
+      <parameter default="20034" name="id_ext_status" type="INT32">
+      <short_desc>Extended status ID</short_desc>
+      <long_desc>Extended status ID</long_desc>
+    <unit></unit>
+    <decimal></decimal>
+      <min>1</min>
+      <max>1023</max>
+    </parameter>
+      <parameter default="50000" name="int_ext_status" type="INT32">
+      <short_desc>Extended status interval (µs)</short_desc>
+      <long_desc>Extended status interval (µs)</long_desc>
+    <unit>µs</unit>
+    <decimal></decimal>
+      <min>0</min>
+      <max>1000000</max>
+    </parameter>
+      <parameter default="50000" name="int_status" type="INT32">
+      <short_desc>ESC status interval (µs)</short_desc>
+      <long_desc>ESC status interval (µs)</long_desc>
+    <unit>µs</unit>
+    <decimal></decimal>
+      <max>1000000</max>
+    </parameter>
+      <parameter default="12" name="mot_i_max" type="FLOAT">
+      <short_desc>Motor current limit in amps</short_desc>
+      <long_desc>Motor current limit in amps. This determines the maximum
+            current controller setpoint, as well as the maximum allowable
+            current setpoint slew rate. This value should generally be set to
+            the continuous current rating listed in the motor’s specification
+            sheet, or set equal to the motor’s specified continuous power
+            divided by the motor voltage limit.</long_desc>
+    <unit>Amps</unit>
+    <decimal>3</decimal>
+      <min>1</min>
+      <max>80</max>
+    </parameter>
+      <parameter default="2300" name="mot_kv" type="INT32">
+      <short_desc>Motor Kv in RPM per volt</short_desc>
+      <long_desc>Motor Kv in RPM per volt. This can be taken from the motor’s
+            specification sheet; accuracy will help control performance but
+            some deviation from the specified value is acceptable.</long_desc>
+    <unit>RPM/v</unit>
+    <decimal>0</decimal>
+      <min>0</min>
+      <max>100</max>
+    </parameter>
+      <parameter default="0.0" name="mot_ls" type="FLOAT">
+      <short_desc>READ ONLY: Motor inductance in henries.</short_desc>
+      <long_desc>READ ONLY: Motor inductance in henries. This is measured on start-up.</long_desc>
+    <unit>henries</unit>
+    <decimal>3</decimal>
+    </parameter>
+      <parameter default="14" name="mot_num_poles" type="INT32">
+      <short_desc>Number of motor poles.</short_desc>
+      <long_desc>Number of motor poles. Used to convert mechanical speeds to
+            electrical speeds. This number should be taken from the motor’s
+            specification sheet.</long_desc>
+    <unit>Poles</unit>
+    <decimal></decimal>
+      <min>2</min>
+      <max>40</max>
+    </parameter>
+      <parameter default="0.0" name="mot_rs" type="FLOAT">
+      <short_desc>READ ONLY: Motor resistance in ohms</short_desc>
+      <long_desc>READ ONLY: Motor resistance in ohms. This is measured on start-up. When
+            tuning a new motor, check that this value is approximately equal
+            to the value shown in the motor’s specification sheet.</long_desc>
+    <unit>Ohms</unit>
+    <decimal>3</decimal>
+    </parameter>
+      <parameter default=".5" name="mot_v_accel" type="INT32">
+      <short_desc>Acceleration limit (V)</short_desc>
+      <long_desc>Acceleration limit (V)</long_desc>
+    <unit>Volts</unit>
+    <decimal>3</decimal>
+      <min>0.01</min>
+      <max>1.00</max>
+    </parameter>
+      <parameter default="14.8" name="mot_v_max" type="FLOAT">
+      <short_desc>Motor voltage limit in volts</short_desc>
+      <long_desc>Motor voltage limit in volts. The current controller’s
+            commanded voltage will never exceed this value. Note that this may
+            safely be above the nominal voltage of the motor; to determine the
+            actual motor voltage limit, divide the motor’s rated power by the
+            motor current limit.</long_desc>
+    <unit>Volts</unit>
+    <decimal>3</decimal>
+      <min>0</min>
+    </parameter>
+  </group>
+</parameters>
@@ -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)
@@ -18,10 +18,14 @@ def indent(elem, level=0):

 class XMLOutput():

-    def __init__(self, groups, board):
+    def __init__(self, groups, board, inject_xml_file_name):
        xml_parameters = ET.Element("parameters")
        xml_version = ET.SubElement(xml_parameters, "version")
        xml_version.text = "3"
+        importtree = ET.parse(inject_xml_file_name)
+        injectgroups = importtree.getroot().findall("group")
+        for igroup in injectgroups:
+            xml_parameters.append(igroup)
        last_param_name = ""
        board_specific_param_set = False
        for group in groups:
@@ -29,7 +29,7 @@ start_name = ""
 end_name = ""

 for group in root:
-	if group.tag == "group":
+	if group.tag == "group" and "no_code_generation" not in group.attrib:
 		header += """
 	/*****************************************************************
 	 * %s
@@ -62,7 +62,8 @@ struct px4_parameters_t px4_parameters = {
 """
 i=0
 for group in root:
-	if group.tag == "group":
+	if group.tag == "group" and "no_code_generation" not in group.attrib:
+
 		src += """
 	/*****************************************************************
 	 * %s
@@ -65,6 +65,12 @@ def main():
                        metavar="FILENAME",
                        help="Create XML file"
                             " (default FILENAME: parameters.xml)")
+    parser.add_argument("-i", "--inject-xml",
+                        nargs='?',
+                        const="../Tools/parameters_injected.xml",
+                        metavar="FILENAME",
+                        help="Inject additional param XML file"
+                             " (default FILENAME: ../Tools/parameters_injected.xml)")
    parser.add_argument("-b", "--board",
                         nargs='?',
                         const="",
@@ -124,7 +130,7 @@ def main():
    # Output to XML file
    if args.xml:
        print("Creating XML file " + args.xml)
-        out = xmlout.XMLOutput(param_groups, args.board)
+        out = xmlout.XMLOutput(param_groups, args.board, args.inject_xml)
        out.Save(args.xml)

    # Output to DokuWiki tables
@@ -40,7 +40,7 @@ Delete all comments and newlines before ROMFS is converted to an image
 """

 from __future__ import print_function
-import argparse
+import argparse, re
 import os


@@ -64,7 +64,7 @@ def main():

                        # read file line by line
                        pruned_content = ""
-                        with open(file_path, "r") as f:
+                        with open(file_path, "rU") as f:
                                for line in f:

                                        # handle mixer files differently than startup files
@@ -76,6 +76,7 @@ def main():
                                                        pruned_content += line
                       # overwrite old scratch file
                        with open(file_path, "wb") as f:
+                                pruned_content = re.sub("\r\n", "\n", pruned_content)
                                f.write(pruned_content.encode("ascii", errors='strict'))


@@ -83,6 +83,12 @@ then
 elif [ "$debugger" == "gdb" ]
 then
 	gdb --args mainapp ../../../../${rc_script}_${program}
+elif [ "$debugger" == "ddd" ]
+then
+	ddd --debugger gdb --args mainapp ../../../../${rc_script}_${program}
+elif [ "$debugger" == "valgrind" ]
+then
+	valgrind ./mainapp ../../../../${rc_script}_${program}
 else
 	./mainapp ../../../../${rc_script}_${program}
 fi
@@ -622,7 +622,7 @@ function(px4_add_common_flags)
 		)

 	list(APPEND added_include_dirs
-		src/lib/eigen
+		src/lib/matrix
 		)

 	set(added_link_dirs) # none used currently
@@ -741,7 +741,7 @@ function(px4_generate_parameters_xml)
 		)
 	add_custom_command(OUTPUT ${OUT}
 		COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_SOURCE_DIR}/Tools/px_process_params.py
-			-s ${path} --board CONFIG_ARCH_${BOARD} --xml
+			-s ${path} --board CONFIG_ARCH_${BOARD} --xml --inject-xml
 		DEPENDS ${param_src_files}
 		)
 	set(${OUT} ${${OUT}} PARENT_SCOPE)
@@ -36,6 +36,7 @@ set(config_module_list
 	modules/ekf_att_pos_estimator
 	modules/position_estimator_inav
 	modules/navigator
+	modules/vtol_att_control
 	modules/mc_pos_control
 	modules/mc_att_control
 	modules/mc_pos_control_multiplatform
@@ -81,6 +81,7 @@ add_message_files(
  offboard_control_mode.msg
  vehicle_force_setpoint.msg
  distance_sensor.msg
+  control_state.msg
 )

 ## Generate services in the 'srv' folder
@@ -135,6 +136,7 @@ include_directories(
  ${CMAKE_BINARY_DIR}/src/modules
  src/
  src/lib
+  src/lib/matrix
  ${EIGEN_INCLUDE_DIRS}
  integrationtests
 )
@@ -0,0 +1,18 @@
+# This is similar to the mavlink message CONTROL_SYSTEM_STATE, but for onboard use */
+uint64 timestamp		# in microseconds since system start
+float32 x_acc			# X acceleration in body frame
+float32 y_acc			# Y acceleration in body frame
+float32 z_acc			# Z acceleration in body frame
+float32 x_vel			# X velocity in body frame
+float32 y_vel			# Y velocity in body frame
+float32 z_vel			# Z velocity in body frame
+float32 x_pos			# X position in local frame
+float32 y_pos			# Y position in local frame
+float32 z_pos			# z position in local frame
+float32 airspeed		# Airspeed, estimated
+float32[3] vel_variance	# Variance in body velocity estimate
+float32[3] pos_variance	# Variance in local position estimate
+float32[4] q 			# Attitude Quaternion
+float32 roll_rate		# Roll body angular rate (rad/s, x forward/y right/z down)
+float32 pitch_rate		# Pitch body angular rate (rad/s, x forward/y right/z down)
+float32 yaw_rate		# Yaw body angular rate (rad/s, x forward/y right/z down)
@@ -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
@@ -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)
@@ -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
@@ -33,7 +33,7 @@ add_custom_target(run_config
 add_dependencies(run_config mainapp)

 foreach(viewer none jmavsim gazebo)
-	foreach(debugger none gdb lldb)
+	foreach(debugger none gdb lldb ddd valgrind)
 		foreach(model none iris vtol)
 			if (debugger STREQUAL "none")
 				if (model STREQUAL "none")
@@ -37,7 +37,6 @@ px4_add_module(
 	SRCS
 		LaunchDetector.cpp
 		CatapultLaunchMethod.cpp
-		launchdetection_params.c
 	DEPENDS
 		platforms__common
 	)
@@ -39,10 +39,6 @@
 * @author Thomas Gubler <thomasgubler@gmail.com>
 */

-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
 * Catapult launch detection parameters, accessible via MAVLink
 *
@@ -49,7 +49,7 @@
 #ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
 #else
-#include "modules/local_position_estimator/matrix/src/Matrix.hpp"
+#include "matrix/math.hpp"
 #endif
 #include <platforms/px4_defines.h>

@@ -339,9 +339,8 @@ public:
 		arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
 		return res;
 #else
-		matrix::Matrix<float, M, N> Me(this->arm_mat.pData);
-		matrix::Matrix<float, M, N> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
-		Matrix<M, N> res(MyInverse.data());
+		matrix::SquareMatrix<float, M> Me = matrix::Matrix<float, M, N>(this->arm_mat.pData);
+		Matrix<M, N> res(Me.I().data());
 		return res;
 #endif
 	}
@@ -54,8 +54,11 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
+#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>

@@ -117,22 +120,28 @@ 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;

 	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;
@@ -140,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;
@@ -168,6 +184,7 @@ private:
 	bool		_data_good = false;
 	bool		_failsafe = false;
 	bool		_vibration_warning = false;
+	bool		_ext_hdg_good = false;

 	int		_mavlink_fd = -1;

@@ -190,18 +207,20 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
 	_voter_mag(3),
 	_lp_roll_rate(250.0f, 18.0f),
 	_lp_pitch_rate(250.0f, 18.0f),
-	_lp_yaw_rate(250, 10.0f)
+	_lp_yaw_rate(250.0f, 10.0f)
 {
 	_voter_mag.set_timeout(200000);

 	_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");
 }

 /**
@@ -269,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));

@@ -374,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);

@@ -431,12 +495,9 @@ void AttitudeEstimatorQ::task_main()
 		att.pitch = euler(1);
 		att.yaw = euler(2);

-		/* the complimentary filter should reflect the true system
-		 * state, but we need smoother outputs for the control system
-		 */
-		att.rollspeed = _lp_roll_rate.apply(_rates(0));
-		att.pitchspeed = _lp_pitch_rate.apply(_rates(1));
-		att.yawspeed = _lp_yaw_rate.apply(_rates(2));
+		att.rollspeed = _rates(0);
+		att.pitchspeed = _rates(1);
+		att.yawspeed = _rates(2);

 		for (int i = 0; i < 3; i++) {
 			att.g_comp[i] = _accel(i) - _pos_acc(i);
@@ -461,6 +522,34 @@ void AttitudeEstimatorQ::task_main()
 		} else {
 			orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
 		}
+
+		struct control_state_s ctrl_state = {};
+
+		ctrl_state.timestamp = sensors.timestamp;
+
+		/* Attitude quaternions for control state */
+		ctrl_state.q[0] = _q(0);
+
+		ctrl_state.q[1] = _q(1);
+
+		ctrl_state.q[2] = _q(2);
+
+		ctrl_state.q[3] = _q(3);
+
+		/* Attitude rates for control state */
+		ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0));
+
+		ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1));
+
+		ctrl_state.yaw_rate = _lp_yaw_rate.apply(_rates(2));
+
+		/* Publish to control state topic */
+		if (_ctrl_state_pub == nullptr) {
+			_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
+
+		} else {
+			orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state);
+		}
 	}
 }

@@ -478,6 +567,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);
@@ -490,6 +580,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);
 	}
 }

@@ -545,12 +636,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
@@ -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.
@@ -180,6 +180,7 @@ 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;
@@ -200,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
@@ -403,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()
@@ -1463,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;
 			}
 		}
@@ -1519,18 +1525,19 @@ int commander_thread_main(int argc, char *argv[])
 				/* copy avionics voltage */
 				status.avionics_power_rail_voltage = system_power.voltage5V_v;

+				/* 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.
 					 */
-					usleep(100000);
 					mavlink_log_critical(mavlink_fd, "USB disconnected, rebooting.")
 					usleep(400000);
 					px4_systemreset(false);
 				}

-				status.usb_connected = system_power.usb_connected;
+				/* finally judge the USB connected state based on software detection */
+				status.usb_connected = _usb_telemetry_active;
 			}
 		}

@@ -3053,6 +3060,8 @@ void *commander_low_prio_loop(void *arg)

 					if (ret != OK) {
 						mavlink_and_console_log_critical(mavlink_fd, "settings auto save error");
+					} else {
+						warnx("settings saved.");
 					}

 					need_param_autosave = false;
@@ -49,6 +49,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_status.h>
@@ -154,12 +155,14 @@ private:
    int     _armedSub;

    orb_advert_t    _att_pub;           /**< vehicle attitude */
+    orb_advert_t    _ctrl_state_pub;        /**< control state */
    orb_advert_t    _global_pos_pub;        /**< global position */
    orb_advert_t    _local_pos_pub;         /**< position in local frame */
    orb_advert_t    _estimator_status_pub;      /**< status of the estimator */
    orb_advert_t    _wind_pub;          /**< wind estimate */

    struct vehicle_attitude_s           _att;           /**< vehicle attitude */
+    struct control_state_s              _ctrl_state;    /**< control state */
    struct gyro_report                  _gyro;
    struct accel_report                 _accel;
    struct mag_report                   _mag;
@@ -320,6 +323,12 @@ private:
    **/
    void publishAttitude();

+    /**
+    * @brief
+    *   Publish the system state for control modules
+    **/
+    void publishControlState();
+
    /**
    * @brief
    *   Publish local position relative to reference point where filter was initialized
@@ -134,12 +134,14 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :

 	/* publications */
 	_att_pub(nullptr),
+	_ctrl_state_pub(nullptr),
 	_global_pos_pub(nullptr),
 	_local_pos_pub(nullptr),
 	_estimator_status_pub(nullptr),
 	_wind_pub(nullptr),

 	_att{},
+	_ctrl_state{},
 	_gyro{},
 	_accel{},
 	_mag{},
@@ -698,6 +700,9 @@ void AttitudePositionEstimatorEKF::task_main()
 					// Publish attitude estimations
 					publishAttitude();

+					// publish control state
+					publishControlState();
+
 					// Publish Local Position estimations
 					publishLocalPosition();

@@ -822,9 +827,9 @@ void AttitudePositionEstimatorEKF::publishAttitude()
 	_att.pitch = euler(1);
 	_att.yaw = euler(2);

-	_att.rollspeed = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
-	_att.pitchspeed = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
-	_att.yawspeed = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
+	_att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt;
+	_att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt;
+	_att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt;

 	// gyro offsets
 	_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
@@ -833,7 +838,7 @@ void AttitudePositionEstimatorEKF::publishAttitude()

 	/* lazily publish the attitude only once available */
 	if (_att_pub != nullptr) {
-		/* publish the attitude setpoint */
+		/* publish the attitude */
 		orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);

 	} else {
@@ -842,6 +847,76 @@ void AttitudePositionEstimatorEKF::publishAttitude()
 	}
 }

+void AttitudePositionEstimatorEKF::publishControlState()
+{
+	/* Accelerations in Body Frame */
+	_ctrl_state.x_acc = _ekf->accel.x;
+	_ctrl_state.y_acc = _ekf->accel.y;
+	_ctrl_state.z_acc = _ekf->accel.z;
+
+	/* Velocity in Body Frame */
+	Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]);
+	Vector3f v_n_var(_ekf->P[4][4], _ekf->P[5][5], _ekf->P[6][6]);
+	Vector3f v_b = _ekf->Tnb * v_n;
+	Vector3f v_b_var = _ekf->Tnb * v_n_var;
+
+	_ctrl_state.x_vel = v_b.x;
+	_ctrl_state.y_vel = v_b.y;
+	_ctrl_state.z_vel = v_b.z;
+
+	_ctrl_state.vel_variance[0] = v_b_var.x;
+	_ctrl_state.vel_variance[1] = v_b_var.y;
+	_ctrl_state.vel_variance[2] = v_b_var.z;
+
+	/* Local Position */
+	_ctrl_state.x_pos = _ekf->states[7];
+	_ctrl_state.y_pos = _ekf->states[8];
+
+	// XXX need to announce change of Z reference somehow elegantly
+	_ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset;
+
+	_ctrl_state.pos_variance[0] = _ekf->P[7][7];
+	_ctrl_state.pos_variance[1] = _ekf->P[8][8];
+	_ctrl_state.pos_variance[2] = _ekf->P[9][9];
+
+	/* Attitude */
+	_ctrl_state.timestamp = _last_sensor_timestamp;
+	_ctrl_state.q[0] = _ekf->states[0];
+	_ctrl_state.q[1] = _ekf->states[1];
+	_ctrl_state.q[2] = _ekf->states[2];
+	_ctrl_state.q[3] = _ekf->states[3];
+
+	/* Airspeed (Groundspeed - Windspeed) */
+	_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] -  _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
+
+	/* Attitude Rates */
+	_ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
+	_ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
+	_ctrl_state.yaw_rate = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
+
+	/* Guard from bad data */
+	if (!PX4_ISFINITE(_ctrl_state.x_vel) ||
+		!PX4_ISFINITE(_ctrl_state.y_vel) ||
+		!PX4_ISFINITE(_ctrl_state.z_vel) ||
+		!PX4_ISFINITE(_ctrl_state.x_pos) ||
+		!PX4_ISFINITE(_ctrl_state.y_pos) ||
+		!PX4_ISFINITE(_ctrl_state.z_pos))
+	{
+		// bad data, abort publication
+		return;
+	}
+
+	/* lazily publish the control state only once available */
+	if (_ctrl_state_pub != nullptr) {
+		/* publish the control state */
+		orb_publish(ORB_ID(control_state), _ctrl_state_pub, &_ctrl_state);
+
+	} else {
+		/* advertise and publish */
+		_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &_ctrl_state);
+	}
+}
+
 void AttitudePositionEstimatorEKF::publishLocalPosition()
 {
 	_local_pos.timestamp = _last_sensor_timestamp;
@@ -1672,8 +1747,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
 			PX4_INFO(".");
 		}

-		PX4_INFO(" ");
-
 		return 0;
 	}

@@ -48,6 +48,8 @@
 #define M_PI_F static_cast<float>(M_PI)
 #endif

+#define MIN_AIRSPEED_MEAS 5.0f
+
 constexpr float EKF_COVARIANCE_DIVERGED = 1.0e8f;

 AttPosEKF::AttPosEKF() :
@@ -1686,7 +1688,7 @@ void AttPosEKF::FuseAirspeed()
    // Calculate the predicted airspeed
    VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
    // Perform fusion of True Airspeed measurement
-    if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
+    if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > MIN_AIRSPEED_MEAS))
    {
        // Calculate observation jacobians
        SH_TAS[0] = 1/(sqrtf(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
@@ -2594,7 +2596,7 @@ void AttPosEKF::CovarianceInit()
    P[13][13] = sq(0.2f*dtIMU);

    //Wind velocities
-    P[14][14] = 0.0f;
+    P[14][14] = 0.01f;
    P[15][15]  = P[14][14];

    //Earth magnetic field
@@ -58,7 +58,6 @@
 #include <drivers/drv_accel.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/airspeed.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
@@ -67,7 +66,7 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/fw_virtual_rates_setpoint.h>
 #include <uORB/topics/mc_virtual_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -125,11 +124,10 @@ private:
 	bool		_task_running;			/**< if true, task is running in its mainloop */
 	int		_control_task;			/**< task handle */

-	int		_att_sub;			/**< vehicle attitude subscription */
+	int		_ctrl_state_sub;	/**< control state subscription */
 	int		_accel_sub;			/**< accelerometer subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
-	int		_airspeed_sub;			/**< airspeed subscription */
 	int		_vcontrol_mode_sub;		/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_sub;			/**< notification of manual control updates */
@@ -144,12 +142,11 @@ private:
 	orb_id_t _rates_sp_id;	// pointer to correct rates setpoint uORB metadata structure
 	orb_id_t _actuators_id;	// pointer to correct actuator controls0 uORB metadata structure

-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct control_state_s				_ctrl_state;	/**< control state */
 	struct accel_report				_accel;			/**< body frame accelerations */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct vehicle_rates_setpoint_s			_rates_sp;	/* attitude rates setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct airspeed_s				_airspeed;		/**< airspeed */
 	struct vehicle_control_mode_s			_vcontrol_mode;		/**< vehicle control mode */
 	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
 	struct actuator_controls_s			_actuators_airframe;	/**< actuator control inputs */
@@ -242,6 +239,11 @@ private:

 	}		_parameter_handles;		/**< handles for interesting parameters */

+	// Rotation matrix and euler angles to extract from control state
+	math::Matrix<3, 3> _R;
+	float _roll;
+	float _pitch;
+	float _yaw;

 	ECL_RollController				_roll_ctrl;
 	ECL_PitchController				_pitch_ctrl;
@@ -269,12 +271,6 @@ private:
 	 */
 	void		vehicle_manual_poll();

-
-	/**
-	 * Check for airspeed updates.
-	 */
-	void		vehicle_airspeed_poll();
-
 	/**
 	 * Check for accel updates.
 	 */
@@ -326,9 +322,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_control_task(-1),

 /* subscriptions */
-	_att_sub(-1),
+	_ctrl_state_sub(-1),
 	_accel_sub(-1),
-	_airspeed_sub(-1),
 	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
 	_manual_sub(-1),
@@ -353,12 +348,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_debug(false)
 {
 	/* safely initialize structs */
-	_att = {};
+	_ctrl_state = {};
 	_accel = {};
 	_att_sp = {};
 	_rates_sp = {};
 	_manual = {};
-	_airspeed = {};
 	_vcontrol_mode = {};
 	_actuators = {};
 	_actuators_airframe = {};
@@ -539,18 +533,6 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 	}
 }

-void
-FixedwingAttitudeControl::vehicle_airspeed_poll()
-{
-	/* check if there is a new position */
-	bool airspeed_updated;
-	orb_check(_airspeed_sub, &airspeed_updated);
-
-	if (airspeed_updated) {
-		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
-	}
-}
-
 void
 FixedwingAttitudeControl::vehicle_accel_poll()
 {
@@ -623,9 +605,8 @@ FixedwingAttitudeControl::task_main()
 	 * do subscriptions
 	 */
 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
 	_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
-	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
@@ -641,7 +622,6 @@ FixedwingAttitudeControl::task_main()
 	parameters_update();

 	/* get an initial update for all sensor and status data */
-	vehicle_airspeed_poll();
 	vehicle_setpoint_poll();
 	vehicle_accel_poll();
 	vehicle_control_mode_poll();
@@ -654,7 +634,7 @@ FixedwingAttitudeControl::task_main()
 	/* Setup of loop */
 	fds[0].fd = _params_sub;
 	fds[0].events = POLLIN;
-	fds[1].fd = _att_sub;
+	fds[1].fd = _ctrl_state_sub;
 	fds[1].events = POLLIN;

 	_task_running = true;
@@ -699,9 +679,19 @@ FixedwingAttitudeControl::task_main()
 				deltaT = 0.01f;

 			/* load local copies */
-			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+			orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);


+			/* get current rotation matrix and euler angles from control state quaternions */
+			math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
+			_R = q_att.to_dcm();
+
+			math::Vector<3> euler_angles;
+			euler_angles = _R.to_euler();
+			_roll    = euler_angles(0);
+			_pitch   = euler_angles(1);
+			_yaw     = euler_angles(2);
+
 			if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
 				/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
 				 *
@@ -719,50 +709,36 @@ FixedwingAttitudeControl::task_main()
 				 * Rxy	Ryy  Rzy		-Rzy  Ryy  Rxy
 				 * Rxz	Ryz  Rzz		-Rzz  Ryz  Rxz
 				 * */
-				math::Matrix<3, 3> R;				//original rotation matrix
-				math::Matrix<3, 3> R_adapted;		//modified rotation matrix
-				R.set(_att.R);
-				R_adapted.set(_att.R);
+				math::Matrix<3, 3> R_adapted = _R;		//modified rotation matrix

 				/* move z to x */
-				R_adapted(0, 0) = R(0, 2);
-				R_adapted(1, 0) = R(1, 2);
-				R_adapted(2, 0) = R(2, 2);
+				R_adapted(0, 0) = _R(0, 2);
+				R_adapted(1, 0) = _R(1, 2);
+				R_adapted(2, 0) = _R(2, 2);

 				/* move x to z */
-				R_adapted(0, 2) = R(0, 0);
-				R_adapted(1, 2) = R(1, 0);
-				R_adapted(2, 2) = R(2, 0);
+				R_adapted(0, 2) = _R(0, 0);
+				R_adapted(1, 2) = _R(1, 0);
+				R_adapted(2, 2) = _R(2, 0);

 				/* change direction of pitch (convert to right handed system) */
 				R_adapted(0, 0) = -R_adapted(0, 0);
 				R_adapted(1, 0) = -R_adapted(1, 0);
 				R_adapted(2, 0) = -R_adapted(2, 0);
-				math::Vector<3> euler_angles;		//adapted euler angles for fixed wing operation
-				euler_angles = R_adapted.to_euler();
+				euler_angles = R_adapted.to_euler();  //adapted euler angles for fixed wing operation

 				/* fill in new attitude data */
-				_att.roll    = euler_angles(0);
-				_att.pitch   = euler_angles(1);
-				_att.yaw     = euler_angles(2);
-				PX4_R(_att.R, 0, 0) = R_adapted(0, 0);
-				PX4_R(_att.R, 0, 1) = R_adapted(0, 1);
-				PX4_R(_att.R, 0, 2) = R_adapted(0, 2);
-				PX4_R(_att.R, 1, 0) = R_adapted(1, 0);
-				PX4_R(_att.R, 1, 1) = R_adapted(1, 1);
-				PX4_R(_att.R, 1, 2) = R_adapted(1, 2);
-				PX4_R(_att.R, 2, 0) = R_adapted(2, 0);
-				PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
-				PX4_R(_att.R, 2, 2) = R_adapted(2, 2);
+				_R = R_adapted;
+				_roll    = euler_angles(0);
+				_pitch   = euler_angles(1);
+				_yaw     = euler_angles(2);

 				/* lastly, roll- and yawspeed have to be swaped */
-				float helper = _att.rollspeed;
-				_att.rollspeed = -_att.yawspeed;
-				_att.yawspeed = helper;
+				float helper = _ctrl_state.roll_rate;
+				_ctrl_state.roll_rate = -_ctrl_state.yaw_rate;
+				_ctrl_state.yaw_rate = helper;
 			}

-			vehicle_airspeed_poll();
-
 			vehicle_setpoint_poll();

 			vehicle_accel_poll();
@@ -813,15 +789,14 @@ FixedwingAttitudeControl::task_main()
 				float airspeed;

 				/* if airspeed is not updating, we assume the normal average speed */
-				if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) ||
-				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
+				if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed)) {
 					airspeed = _parameters.airspeed_trim;
 					if (nonfinite) {
 						perf_count(_nonfinite_input_perf);
 					}
 				} else {
 					/* prevent numerical drama by requiring 0.5 m/s minimal speed */
-					airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
+					airspeed = math::max(0.5f, _ctrl_state.airspeed);
 				}

 				/*
@@ -866,7 +841,7 @@ FixedwingAttitudeControl::task_main()
 					/* the pilot does not want to change direction,
 					 * take straight attitude setpoint from position controller
 					 */
-					if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
+					if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) {
 						roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
 					} else {
 						roll_sp = (_manual.y * _parameters.man_roll_max)
@@ -957,27 +932,18 @@ FixedwingAttitudeControl::task_main()
 				}

 				/* Prepare speed_body_u and speed_body_w */
-				float speed_body_u = 0.0f;
-				float speed_body_v = 0.0f;
-				float speed_body_w = 0.0f;
-				if(_att.R_valid) 	{
-					speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d;
-					speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d;
-					speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d;
-				} else	{
-					if (_debug && loop_counter % 10 == 0) {
-						warnx("Did not get a valid R\n");
-					}
-				}
+				float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d;
+				float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d;
+				float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d;

 				/* Prepare data for attitude controllers */
 				struct ECL_ControlData control_input = {};
-				control_input.roll = _att.roll;
-				control_input.pitch = _att.pitch;
-				control_input.yaw = _att.yaw;
-				control_input.roll_rate = _att.rollspeed;
-				control_input.pitch_rate = _att.pitchspeed;
-				control_input.yaw_rate = _att.yawspeed;
+				control_input.roll = _roll;
+				control_input.pitch = _pitch;
+				control_input.yaw = _yaw;
+				control_input.roll_rate = _ctrl_state.roll_rate;
+				control_input.pitch_rate = _ctrl_state.pitch_rate;
+				control_input.yaw_rate = _ctrl_state.yaw_rate;
 				control_input.speed_body_u = speed_body_u;
 				control_input.speed_body_v = speed_body_v;
 				control_input.speed_body_w = speed_body_w;
@@ -1100,9 +1066,9 @@ FixedwingAttitudeControl::task_main()

 			/* lazily publish the setpoint only once available */
 			_actuators.timestamp = hrt_absolute_time();
-			_actuators.timestamp_sample = _att.timestamp;
+			_actuators.timestamp_sample = _ctrl_state.timestamp;
 			_actuators_airframe.timestamp = hrt_absolute_time();
-			_actuators_airframe.timestamp_sample = _att.timestamp;
+			_actuators_airframe.timestamp_sample = _ctrl_state.timestamp;

 			/* Only publish if any of the proper modes are enabled */
 			if(_vcontrol_mode.flag_control_rates_enabled ||
@@ -68,14 +68,13 @@
 #include <drivers/drv_accel.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
-#include <uORB/topics/airspeed.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/navigation_capabilities.h>
 #include <uORB/topics/sensor_combined.h>
@@ -153,8 +152,7 @@ private:

 	int		_global_pos_sub;
 	int		_pos_sp_triplet_sub;
-	int		_att_sub;			/**< vehicle attitude subscription */
-	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_ctrl_state_sub;			/**< control state subscription */
 	int		_control_mode_sub;		/**< control mode subscription */
 	int		_vehicle_status_sub;		/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
@@ -165,11 +163,10 @@ private:
 	orb_advert_t	_tecs_status_pub;		/**< TECS status publication */
 	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */

-	struct vehicle_attitude_s			_att;				/**< vehicle attitude */
+	struct control_state_s				_ctrl_state;			/**< control state */
 	struct vehicle_attitude_setpoint_s		_att_sp;			/**< vehicle attitude setpoint */
 	struct navigation_capabilities_s		_nav_capabilities;		/**< navigation capabilities */
 	struct manual_control_setpoint_s		_manual;			/**< r/c channel data */
-	struct airspeed_s				_airspeed;			/**< airspeed */
 	struct vehicle_control_mode_s			_control_mode;			/**< control mode */
 	struct vehicle_status_s				_vehicle_status;		/**< vehicle status */
 	struct vehicle_global_position_s		_global_pos;			/**< global vehicle position */
@@ -222,6 +219,9 @@ private:
 	float _groundspeed_undershoot;			///< ground speed error to min. speed in m/s
 	bool _global_pos_valid;				///< global position is valid
 	math::Matrix<3, 3> _R_nb;			///< current attitude
+	float _roll;
+	float _pitch;
+	float _yaw;

 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
@@ -356,14 +356,9 @@ private:
 	bool		vehicle_manual_control_setpoint_poll();

 	/**
-	 * Check for airspeed updates.
+	 * Check for changes in control state.
 	 */
-	bool		vehicle_airspeed_poll();
-
-	/**
-	 * Check for position updates.
-	 */
-	void		vehicle_attitude_poll();
+	void		control_state_poll();

 	/**
 	 * Check for accel updates.
@@ -481,8 +476,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 /* subscriptions */
 	_global_pos_sub(-1),
 	_pos_sp_triplet_sub(-1),
-	_att_sub(-1),
-	_airspeed_sub(-1),
+	_ctrl_state_sub(-1),
 	_control_mode_sub(-1),
 	_vehicle_status_sub(-1),
 	_params_sub(-1),
@@ -495,11 +489,10 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_nav_capabilities_pub(nullptr),

 /* states */
-	_att(),
+	_ctrl_state(),
 	_att_sp(),
 	_nav_capabilities(),
 	_manual(),
-	_airspeed(),
 	_control_mode(),
 	_vehicle_status(),
 	_global_pos(),
@@ -749,32 +742,6 @@ FixedwingPositionControl::vehicle_status_poll()
 	}
 }

-bool
-FixedwingPositionControl::vehicle_airspeed_poll()
-{
-	/* check if there is an airspeed update or if it timed out */
-	bool airspeed_updated;
-	orb_check(_airspeed_sub, &airspeed_updated);
-
-	if (airspeed_updated) {
-		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
-		_airspeed_valid = true;
-		_airspeed_last_valid = hrt_absolute_time();
-
-	} else {
-
-		/* no airspeed updates for one second */
-		if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
-			_airspeed_valid = false;
-		}
-	}
-
-	/* update TECS state */
-	_tecs.enable_airspeed(_airspeed_valid);
-
-	return airspeed_updated;
-}
-
 bool
 FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
 {
@@ -790,21 +757,38 @@ FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
 	return manual_updated;
 }

-
 void
-FixedwingPositionControl::vehicle_attitude_poll()
+FixedwingPositionControl::control_state_poll()
 {
 	/* check if there is a new position */
-	bool att_updated;
-	orb_check(_att_sub, &att_updated);
+	bool ctrl_state_updated;
+	orb_check(_ctrl_state_sub, &ctrl_state_updated);

-	if (att_updated) {
-		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+	if (ctrl_state_updated) {
+		orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
+		_airspeed_valid = true;
+		_airspeed_last_valid = hrt_absolute_time();

-		/* set rotation matrix */
-		for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-				_R_nb(i, j) = PX4_R(_att.R, i, j);
+	} else {
+
+		/* no airspeed updates for one second */
+		if (_airspeed_valid && (hrt_absolute_time() - _airspeed_last_valid) > 1e6) {
+			_airspeed_valid = false;
+		}
 	}
+
+	/* set rotation matrix and euler angles */
+	math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
+	_R_nb = q_att.to_dcm();
+
+	math::Vector<3> euler_angles;
+	euler_angles = _R_nb.to_euler();
+	_roll    = euler_angles(0);
+	_pitch   = euler_angles(1);
+	_yaw     = euler_angles(2);
+	
+	/* update TECS state */
+	_tecs.enable_airspeed(_airspeed_valid);
 }

 void
@@ -853,7 +837,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand)
 	float airspeed;

 	if (_airspeed_valid) {
-		airspeed = _airspeed.true_airspeed_m_s;
+		airspeed = _ctrl_state.airspeed;

 	} else {
 		airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) / 2.0f;
@@ -1084,7 +1068,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi

 	/* update TECS filters */
 	if (!_mTecs.getEnabled()) {
-		_tecs.update_state(_global_pos.alt, _airspeed.indicated_airspeed_m_s, _R_nb,
+		_tecs.update_state(_global_pos.alt, _ctrl_state.airspeed, _R_nb,
 			accel_body, accel_earth, (_global_pos.timestamp > 0), in_air_alt_control);
 	}

@@ -1117,7 +1101,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			/* reset integrators */
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
-				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+				_mTecs.resetDerivatives(_ctrl_state.airspeed);
 			} else {
 				_tecs.reset_state();
 			}
@@ -1127,7 +1111,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* reset hold altitude */
 		_hold_alt = _global_pos.alt;
 		/* reset hold yaw */
-		_hdg_hold_yaw = _att.yaw;
+		_hdg_hold_yaw = _yaw;

 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
@@ -1221,14 +1205,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					if (pos_sp_triplet.previous.valid) {
 						target_bearing = bearing_lastwp_currwp;
 					} else {
-						target_bearing = _att.yaw;
+						target_bearing = _yaw;
 					}
 					mavlink_log_info(_mavlink_fd, "#audio: Landing, heading hold");
 				}

-//					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
+//					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw));

-				_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d);
+				_l1_control.navigate_heading(target_bearing, _yaw, ground_speed_2d);

 				land_noreturn_horizontal = true;

@@ -1469,7 +1453,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
 			/* Need to init because last loop iteration was in a different mode */
 			_hold_alt = _global_pos.alt;
-			_hdg_hold_yaw = _att.yaw;
+			_hdg_hold_yaw = _yaw;
 			_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
 			_yaw_lock_engaged = false;
 		}
@@ -1478,7 +1462,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			/* reset integrators */
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
-				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+				_mTecs.resetDerivatives(_ctrl_state.airspeed);
 			} else {
 				_tecs.reset_state();
 			}
@@ -1524,7 +1508,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi

 		if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) {
 			/* heading / roll is zero, lock onto current heading */
-			if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
+			if (fabsf(_ctrl_state.yaw_rate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
 				// little yaw movement, lock to current heading
 				_yaw_lock_engaged = true;

@@ -1543,7 +1527,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				/* just switched back from non heading-hold to heading hold */
 				if (!_hdg_hold_enabled) {
 					_hdg_hold_enabled = true;
-					_hdg_hold_yaw = _att.yaw;
+					_hdg_hold_yaw = _yaw;

 					get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
 				}
@@ -1591,7 +1575,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			/* reset integrators */
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
-				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+				_mTecs.resetDerivatives(_ctrl_state.airspeed);
 			} else {
 				_tecs.reset_state();
 			}
@@ -1704,11 +1688,10 @@ FixedwingPositionControl::task_main()
 	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
-	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));

@@ -1785,10 +1768,9 @@ FixedwingPositionControl::task_main()
 			// XXX add timestamp check
 			_global_pos_valid = true;

-			vehicle_attitude_poll();
+			control_state_poll();
 			vehicle_setpoint_poll();
 			vehicle_sensor_combined_poll();
-			vehicle_airspeed_poll();
 			vehicle_manual_control_setpoint_poll();
 			// vehicle_baro_poll();

@@ -1894,7 +1876,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 			/* use pitch max set by MT param */
 			limitOverride.disablePitchMaxOverride();
 		}
-		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
+		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _ctrl_state.airspeed, v_sp, mode,
 				limitOverride);
 	} else {
 		if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
@@ -1907,8 +1889,8 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		_tecs.set_detect_underspeed_enabled(!(mode == tecs_status_s::TECS_MODE_LAND || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM));

 		/* Using tecs library */
-		_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
-					    _airspeed.indicated_airspeed_m_s, eas2tas,
+		_tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp,
+					    _ctrl_state.airspeed, eas2tas,
 					    climbout_mode, climbout_pitch_min_rad,
 					    throttle_min, throttle_max, throttle_cruise,
 					    pitch_min_rad, pitch_max_rad);
@@ -811,7 +811,7 @@ void BlockLocalPositionEstimator::predict()
 	Q(X_bz, X_bz) = _pn_b_noise_power.get();

 	// continuous time kalman filter prediction
-	Matrix<float, n_x, 1> dx = (A * _x + B * _u) * getDt();
+	Vector<float, n_x> dx = (A * _x + B * _u) * getDt();

 	// only predict for components we have
 	// valid measurements for
@@ -904,16 +904,16 @@ void BlockLocalPositionEstimator::correctFlow()
 	_flowY += global_speed[1] * dt;

 	// measurement
-	Vector2f y;
+	Vector<float, 2> y;
 	y(0) = _flowX;
 	y(1) = _flowY;

 	// residual
-	Vector2f r = y - C * _x;
+	Vector<float, 2> r = y - C * _x;

 	// residual covariance, (inverse)
 	Matrix<float, n_y_flow, n_y_flow> S_I =
-		(C * _P * C.transpose() + R).inverse();
+		inv<float, n_y_flow>(C * _P * C.transpose() + R);

 	// fault detection
 	float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -981,17 +981,17 @@ void BlockLocalPositionEstimator::correctSonar()
 	}

 	// measurement
-	Matrix<float, n_y_sonar, 1> y;
+	Vector<float, n_y_sonar> y;
 	y(0) = (d - _sonarAltHome) *
 	       cosf(_sub_att.get().roll) *
 	       cosf(_sub_att.get().pitch);

 	// residual
-	Matrix<float, n_y_sonar, 1> r = y - C * _x;
+	Vector<float, n_y_sonar> r = y - C * _x;

 	// residual covariance, (inverse)
 	Matrix<float, n_y_sonar, n_y_sonar> S_I =
-		(C * _P * C.transpose() + R).inverse();
+		inv<float, n_y_sonar>(C * _P * C.transpose() + R);

 	// fault detection
 	float beta = sqrtf((r.transpose()  * (S_I * r))(0, 0));
@@ -1032,7 +1032,7 @@ void BlockLocalPositionEstimator::correctSonar()
 void BlockLocalPositionEstimator::correctBaro()
 {

-	Matrix<float, n_y_baro, 1> y;
+	Vector<float, n_y_baro> y;
 	y(0) = _sub_sensor.get().baro_alt_meter[0] - _baroAltHome;

 	// baro measurement matrix
@@ -1046,8 +1046,8 @@ void BlockLocalPositionEstimator::correctBaro()

 	// residual
 	Matrix<float, n_y_baro, n_y_baro> S_I =
-		((C * _P * C.transpose()) + R).inverse();
-	Matrix<float, n_y_baro, 1> r = y - (C * _x);
+		inv<float, n_y_baro>((C * _P * C.transpose()) + R);
+	Vector<float, n_y_baro> r = y - (C * _x);

 	// fault detection
 	float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1060,7 +1060,7 @@ void BlockLocalPositionEstimator::correctBaro()
 		}

 		// lower baro trust
-		S_I = ((C * _P * C.transpose()) + R * 10).inverse();
+		S_I = inv<float, n_y_baro>((C * _P * C.transpose()) + R * 10);

 	} else if (_baroFault) {
 		_baroFault = FAULT_NONE;
@@ -1104,15 +1104,15 @@ void BlockLocalPositionEstimator::correctLidar()
 		R(0, 0) = cov;
 	}

-	Matrix<float, n_y_lidar, 1> y;
+	Vector<float, n_y_lidar> y;
 	y.setZero();
 	y(0) = (d - _lidarAltHome) *
 	       cosf(_sub_att.get().roll) *
 	       cosf(_sub_att.get().pitch);

 	// residual
-	Matrix<float, n_y_lidar, n_y_lidar> S_I = ((C * _P * C.transpose()) + R).inverse();
-	Matrix<float, n_y_lidar, 1> r = y - C * _x;
+	Matrix<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>((C * _P * C.transpose()) + R);
+	Vector<float, n_y_lidar> r = y - C * _x;

 	// fault detection
 	float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1166,7 +1166,7 @@ void BlockLocalPositionEstimator::correctGps()  	// TODO : use another other met
 	//printf("home: lat %10g, lon, %10g alt %10g\n", _sub_home.lat, _sub_home.lon, double(_sub_home.alt));
 	//printf("local: x %10g y %10g z %10g\n", double(px), double(py), double(pz));

-	Matrix<float, 6, 1> y;
+	Vector<float, 6> y;
 	y.setZero();
 	y(0) = px;
 	y(1) = py;
@@ -1213,8 +1213,8 @@ void BlockLocalPositionEstimator::correctGps()  	// TODO : use another other met
 	R(5, 5) = var_vz;

 	// residual
-	Matrix<float, 6, 1> r = y - C * _x;
-	Matrix<float, 6, 6> S_I = (C * _P * C.transpose() + R).inverse();
+	Vector<float, 6> r = y - C * _x;
+	Matrix<float, 6, 6> S_I = inv<float, 6>(C * _P * C.transpose() + R);

 	// fault detection
 	float beta = sqrtf((r.transpose() * (S_I * r))(0, 0));
@@ -1245,7 +1245,7 @@ void BlockLocalPositionEstimator::correctGps()  	// TODO : use another other met
 		}

 		// trust GPS less
-		S_I = ((C * _P * C.transpose()) + R * 10).inverse();
+		S_I = inv<float, 6>((C * _P * C.transpose()) + R * 10);

 	} else if (_gpsFault) {
 		_gpsFault = FAULT_NONE;
@@ -1266,7 +1266,7 @@ void BlockLocalPositionEstimator::correctGps()  	// TODO : use another other met
 void BlockLocalPositionEstimator::correctVision()
 {

-	Matrix<float, 3, 1> y;
+	Vector<float, 3> y;
 	y.setZero();
 	y(0) = _sub_vision_pos.get().x - _visionHome(0);
 	y(1) = _sub_vision_pos.get().y - _visionHome(1);
@@ -1287,7 +1287,7 @@ void BlockLocalPositionEstimator::correctVision()
 	R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get();

 	// residual
-	Matrix<float, n_y_vision, n_y_vision> S_I = ((C * _P * C.transpose()) + R).inverse();
+	Matrix<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R);
 	Matrix<float, n_y_vision, 1> r = y - C * _x;

 	// fault detection
@@ -1301,7 +1301,7 @@ void BlockLocalPositionEstimator::correctVision()
 		}

 		// trust less
-		S_I = ((C * _P * C.transpose()) + R * 10).inverse();
+		S_I = inv<float, n_y_vision>((C * _P * C.transpose()) + R * 10);

 	} else if (_visionFault) {
 		_visionFault = FAULT_NONE;
@@ -1322,7 +1322,7 @@ void BlockLocalPositionEstimator::correctVision()
 void BlockLocalPositionEstimator::correctmocap()
 {

-	Matrix<float, n_y_mocap, 1> y;
+	Vector<float, n_y_mocap> y;
 	y.setZero();
 	y(Y_mocap_x) = _sub_mocap.get().x - _mocapHome(0);
 	y(Y_mocap_y) = _sub_mocap.get().y - _mocapHome(1);
@@ -1345,7 +1345,7 @@ void BlockLocalPositionEstimator::correctmocap()
 	R(Y_mocap_z, Y_mocap_z) = mocap_p_var;

 	// residual
-	Matrix<float, n_y_mocap, n_y_mocap> S_I = ((C * _P * C.transpose()) + R).inverse();
+	Matrix<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R);
 	Matrix<float, n_y_mocap, 1> r = y - C * _x;

 	// fault detection
@@ -1359,7 +1359,7 @@ void BlockLocalPositionEstimator::correctmocap()
 		}

 		// trust less
-		S_I = ((C * _P * C.transpose()) + R * 10).inverse();
+		S_I = inv<float, n_y_mocap>((C * _P * C.transpose()) + R * 10);

 	} else if (_mocapFault) {
 		_mocapFault = FAULT_NONE;
@@ -6,7 +6,7 @@
 #include <lib/geo/geo.h>

 #ifdef USE_MATRIX_LIB
-#include "matrix/src/Matrix.hpp"
+#include "matrix/Matrix.hpp"
 using namespace matrix;
 #else
 #include <Eigen/Eigen>
@@ -304,7 +304,7 @@ private:
 	perf_counter_t _err_perf;

 	// state space
-	Matrix<float, n_x, 1>  _x; // state vector
-	Matrix<float, n_u, 1>  _u; // input vector
+	Vector<float, n_x>  _x; // state vector
+	Vector<float, n_u>  _u; // input vector
 	Matrix<float, n_x, n_x>  _P; // state covariance matrix
 };
@@ -34,8 +34,6 @@ if(${OS} STREQUAL "nuttx")
 	list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500)
 elseif(${OS} STREQUAL "posix")
 	list(APPEND MODULE_CFLAGS -Wno-error)
-	# add matrix tests
-	add_subdirectory(matrix)
 endif()

 # use custom matrix lib instead of Eigen
@@ -1 +0,0 @@
-build*/
@@ -1,13 +0,0 @@
-cmake_minimum_required(VERSION 2.8)
-
-project(matrix CXX)
-
-set(CMAKE_BUILD_TYPE "RelWithDebInfo")
-
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++")
-
-enable_testing()
-
-include_directories(src)
-
-add_subdirectory(test)
@@ -1,464 +0,0 @@
-#pragma once
-
-#include <stdio.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
-
-namespace matrix
-{
-
-template<typename T, size_t  M, size_t N>
-class Matrix
-{
-
-private:
-	T _data[M][N];
-	size_t _rows;
-	size_t _cols;
-
-public:
-
-	Matrix() :
-		_data(),
-		_rows(M),
-		_cols(N)
-	{
-	}
-
-	Matrix(const T *data) :
-		_data(),
-		_rows(M),
-		_cols(N)
-	{
-		memcpy(_data, data, sizeof(_data));
-	}
-
-	Matrix(const Matrix &other) :
-		_data(),
-		_rows(M),
-		_cols(N)
-	{
-		memcpy(_data, other._data, sizeof(_data));
-	}
-
-	Matrix(T x, T y, T z) :
-		_data(),
-		_rows(M),
-		_cols(N)
-	{
-		// TODO, limit to only 3x1 matrices
-		_data[0][0] = x;
-		_data[1][0] = y;
-		_data[2][0] = z;
-	}
-
-	/**
-	 * Accessors/ Assignment etc.
-	 */
-
-	T *data()
-	{
-		return _data[0];
-	}
-
-	inline size_t rows() const
-	{
-		return _rows;
-	}
-
-	inline size_t cols() const
-	{
-		return _rows;
-	}
-
-	inline T operator()(size_t i) const
-	{
-		return _data[i][0];
-	}
-
-	inline T operator()(size_t i, size_t j) const
-	{
-		return _data[i][j];
-	}
-
-	inline T &operator()(size_t i)
-	{
-		return _data[i][0];
-	}
-
-	inline T &operator()(size_t i, size_t j)
-	{
-		return _data[i][j];
-	}
-
-	/**
-	 * Matrix Operations
-	 */
-
-	// this might use a lot of programming memory
-	// since it instantiates a class for every
-	// required mult pair, but it provides
-	// compile time size_t checking
-	template<size_t P>
-	Matrix<T, M, P> operator*(const Matrix<T, N, P> &other) const
-	{
-		const Matrix<T, M, N> &self = *this;
-		Matrix<T, M, P> res;
-		res.setZero();
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t k = 0; k < P; k++) {
-				for (size_t j = 0; j < N; j++) {
-					res(i, k) += self(i, j) * other(j, k);
-				}
-			}
-		}
-
-		return res;
-	}
-
-	Matrix<T, M, N> operator+(const Matrix<T, M, N> &other) const
-	{
-		Matrix<T, M, N> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(i , j) = self(i, j) + other(i, j);
-			}
-		}
-
-		return res;
-	}
-
-	bool operator==(const Matrix<T, M, N> &other) const
-	{
-		Matrix<T, M, N> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				if (self(i , j) != other(i, j)) {
-					return false;
-				}
-			}
-		}
-
-		return true;
-	}
-
-	Matrix<T, M, N> operator-(const Matrix<T, M, N> &other) const
-	{
-		Matrix<T, M, N> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(i , j) = self(i, j) - other(i, j);
-			}
-		}
-
-		return res;
-	}
-
-	void operator+=(const Matrix<T, M, N> &other)
-	{
-		Matrix<T, M, N> &self = *this;
-		self = self + other;
-	}
-
-	void operator-=(const Matrix<T, M, N> &other)
-	{
-		Matrix<T, M, N> &self = *this;
-		self = self - other;
-	}
-
-	void operator*=(const Matrix<T, M, N> &other)
-	{
-		Matrix<T, M, N> &self = *this;
-		self = self * other;
-	}
-
-	/**
-	 * Scalar Operations
-	 */
-
-	Matrix<T, M, N> operator*(T scalar) const
-	{
-		Matrix<T, M, N> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(i , j) = self(i, j) * scalar;
-			}
-		}
-
-		return res;
-	}
-
-	Matrix<T, M, N> operator+(T scalar) const
-	{
-		Matrix<T, M, N> res;
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(i , j) = self(i, j) + scalar;
-			}
-		}
-
-		return res;
-	}
-
-	void operator*=(T scalar)
-	{
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				self(i, j) = self(i, j) * scalar;
-			}
-		}
-	}
-
-	void operator/=(T scalar)
-	{
-		Matrix<T, M, N> &self = *this;
-		self = self * (1.0f / scalar);
-	}
-
-	/**
-	 * Misc. Functions
-	 */
-
-	void print()
-	{
-		Matrix<T, M, N> &self = *this;
-		printf("\n");
-
-		for (size_t i = 0; i < M; i++) {
-			printf("[");
-
-			for (size_t j = 0; j < N; j++) {
-				printf("%10g\t", double(self(i, j)));
-			}
-
-			printf("]\n");
-		}
-	}
-
-	Matrix<T, N, M> transpose() const
-	{
-		Matrix<T, N, M> res;
-		const Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			for (size_t j = 0; j < N; j++) {
-				res(j, i) = self(i, j);
-			}
-		}
-
-		return res;
-	}
-
-	Matrix<T, M, M> expm(float dt, size_t n) const
-	{
-		Matrix<float, M, M> res;
-		res.setIdentity();
-		Matrix<float, M, M> A_pow = *this;
-		float k_fact = 1;
-		size_t k = 1;
-
-		while (k < n) {
-			res += A_pow * (float(pow(dt, k)) / k_fact);
-
-			if (k == n) { break; }
-
-			A_pow *= A_pow;
-			k_fact *= k;
-			k++;
-		}
-
-		return res;
-	}
-
-	Matrix<T, M, 1> diagonal() const
-	{
-		Matrix<T, M, 1> res;
-		// force square for now
-		const Matrix<T, M, M> &self = *this;
-
-		for (size_t i = 0; i < M; i++) {
-			res(i) = self(i, i);
-		}
-
-		return res;
-	}
-
-	void setZero()
-	{
-		memset(_data, 0, sizeof(_data));
-	}
-
-	void setIdentity()
-	{
-		setZero();
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < M and i < N; i++) {
-			self(i, i) = 1;
-		}
-	}
-
-	inline void swapRows(size_t a, size_t b)
-	{
-		if (a == b) { return; }
-
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t j = 0; j < cols(); j++) {
-			T tmp = self(a, j);
-			self(a, j) = self(b, j);
-			self(b, j) = tmp;
-		}
-	}
-
-	inline void swapCols(size_t a, size_t b)
-	{
-		if (a == b) { return; }
-
-		Matrix<T, M, N> &self = *this;
-
-		for (size_t i = 0; i < rows(); i++) {
-			T tmp = self(i, a);
-			self(i, a) = self(i, b);
-			self(i, b) = tmp;
-		}
-	}
-
-	/**
-	 * inverse based on LU factorization with partial pivotting
-	 */
-	Matrix <T, M, M> inverse() const
-	{
-		Matrix<T, M, M> L;
-		L.setIdentity();
-		const Matrix<T, M, M> &A = (*this);
-		Matrix<T, M, M> U = A;
-		Matrix<T, M, M> P;
-		P.setIdentity();
-
-		//printf("A:\n"); A.print();
-
-		// for all diagonal elements
-		for (size_t n = 0; n < N; n++) {
-
-			// if diagonal is zero, swap with row below
-			if (fabsf(U(n, n)) < 1e-8f) {
-				//printf("trying pivot for row %d\n",n);
-				for (size_t i = 0; i < N; i++) {
-					if (i == n) { continue; }
-
-					//printf("\ttrying row %d\n",i);
-					if (fabsf(U(i, n)) > 1e-8f) {
-						//printf("swapped %d\n",i);
-						U.swapRows(i, n);
-						P.swapRows(i, n);
-					}
-				}
-			}
-
-#ifdef MATRIX_ASSERT
-			//printf("A:\n"); A.print();
-			//printf("U:\n"); U.print();
-			//printf("P:\n"); P.print();
-			//fflush(stdout);
-			ASSERT(fabsf(U(n, n)) > 1e-8f);
-#endif
-
-			// failsafe, return zero matrix
-			if (fabsf(U(n, n)) < 1e-8f) {
-				Matrix<T, M, M> zero;
-				zero.setZero();
-				return zero;
-			}
-
-			// for all rows below diagonal
-			for (size_t i = (n + 1); i < N; i++) {
-				L(i, n) = U(i, n) / U(n, n);
-
-				// add i-th row and n-th row
-				// multiplied by: -a(i,n)/a(n,n)
-				for (size_t k = n; k < N; k++) {
-					U(i, k) -= L(i, n) * U(n, k);
-				}
-			}
-		}
-
-		//printf("L:\n"); L.print();
-		//printf("U:\n"); U.print();
-
-		// solve LY=P*I for Y by forward subst
-		Matrix<T, M, M> Y = P;
-
-		// for all columns of Y
-		for (size_t c = 0; c < N; c++) {
-			// for all rows of L
-			for (size_t i = 0; i < N; i++) {
-				// for all columns of L
-				for (size_t j = 0; j < i; j++) {
-					// for all existing y
-					// subtract the component they
-					// contribute to the solution
-					Y(i, c) -= L(i, j) * Y(j, c);
-				}
-
-				// divide by the factor
-				// on current
-				// term to be solved
-				// Y(i,c) /= L(i,i);
-				// but L(i,i) = 1.0
-			}
-		}
-
-		//printf("Y:\n"); Y.print();
-
-		// solve Ux=y for x by back subst
-		Matrix<T, M, M> X = Y;
-
-		// for all columns of X
-		for (size_t c = 0; c < N; c++) {
-			// for all rows of U
-			for (size_t k = 0; k < N; k++) {
-				// have to go in reverse order
-				size_t i = N - 1 - k;
-
-				// for all columns of U
-				for (size_t j = i + 1; j < N; j++) {
-					// for all existing x
-					// subtract the component they
-					// contribute to the solution
-					X(i, c) -= U(i, j) * X(j, c);
-				}
-
-				// divide by the factor
-				// on current
-				// term to be solved
-				X(i, c) /= U(i, i);
-			}
-		}
-
-		//printf("X:\n"); X.print();
-		return X;
-	}
-
-};
-
-typedef Matrix<float, 2, 1> Vector2f;
-typedef Matrix<float, 3, 1> Vector3f;
-typedef Matrix<float, 3, 3> Matrix3f;
-
-}; // namespace matrix
@@ -1,15 +0,0 @@
-set(tests
-	setIdentity
-	inverse
-	matrixMult
-	vectorAssignment
-	matrixAssignment
-	matrixScalarMult
-	transpose
-	)
-
-foreach(test ${tests})
-	add_executable(${test}
-		${test}.cpp)
-	add_test(${test} ${test})
-endforeach()
@@ -1,30 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-#include <stdio.h>
-
-using namespace matrix;
-
-int main()
-{
-	float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
-	Matrix3f A(data);
-	Matrix3f A_I = A.inverse();
-	float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
-	Matrix3f A_I_check(data_check);
-	(void)A_I;
-	assert(A_I == A_I_check);
-
-	// stess test
-	static const size_t n = 100;
-	Matrix<float, n, n> A_large;
-	A_large.setIdentity();
-	Matrix<float, n, n> A_large_I;
-	A_large_I.setZero();
-
-	for (size_t i = 0; i < 100; i++) {
-		A_large_I = A_large.inverse();
-		assert(A_large == A_large_I);
-	}
-
-	return 0;
-}
@@ -1,29 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-
-using namespace matrix;
-
-int main()
-{
-	Matrix3f m;
-	m.setZero();
-	m(0, 0) = 1;
-	m(0, 1) = 2;
-	m(0, 2) = 3;
-	m(1, 0) = 4;
-	m(1, 1) = 5;
-	m(1, 2) = 6;
-	m(2, 0) = 7;
-	m(2, 1) = 8;
-	m(2, 2) = 9;
-
-	m.print();
-
-	float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
-	Matrix3f m2(data);
-	m2.print();
-
-	assert(m == m2);
-
-	return 0;
-}
@@ -1,26 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-#include <stdio.h>
-
-using namespace matrix;
-
-int main()
-{
-	float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1};
-	Matrix3f A(data);
-	float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1};
-	Matrix3f A_I(data_check);
-	Matrix3f I;
-	I.setIdentity();
-	A.print();
-	A_I.print();
-	Matrix3f R = A * A_I;
-	R.print();
-	assert(R == I);
-
-	Matrix3f R2 = A;
-	R2 *= A_I;
-	R2.print();
-	assert(R2 == I);
-	return 0;
-}
@@ -1,18 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-#include <stdio.h>
-
-using namespace matrix;
-
-int main()
-{
-	float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9};
-	Matrix3f A(data);
-	A = A * 2;
-	float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18};
-	Matrix3f A_check(data_check);
-	A.print();
-	A_check.print();
-	assert(A == A_check);
-	return 0;
-}
@@ -1,25 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-
-using namespace matrix;
-
-int main()
-{
-	Matrix3f A;
-	A.setIdentity();
-	assert(A.rows() == 3);
-	assert(A.cols() == 3);
-
-	for (int i = 0; i < 3; i++) {
-		for (int j = 0; j < 3; j++) {
-			if (i == j) {
-				assert(A(i, j) == 1);
-
-			} else {
-				assert(A(i, j) == 0);
-			}
-		}
-	}
-
-	return 0;
-}
@@ -1,18 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-#include <stdio.h>
-
-using namespace matrix;
-
-int main()
-{
-	float data[9] = {1, 2, 3, 4, 5, 6};
-	Matrix<float, 2, 3> A(data);
-	Matrix<float, 3, 2> A_T = A.transpose();
-	float data_check[9] = {1, 4, 2, 5, 3, 6};
-	Matrix<float, 3, 2> A_T_check(data_check);
-	A_T.print();
-	A_T_check.print();
-	assert(A_T == A_T_check);
-	return 0;
-}
@@ -1,28 +0,0 @@
-#include "Matrix.hpp"
-#include <assert.h>
-
-using namespace matrix;
-
-int main()
-{
-	Vector3f v;
-	v(0) = 1;
-	v(1) = 2;
-	v(2) = 3;
-
-	v.print();
-
-	assert(v(0) == 1);
-	assert(v(1) == 2);
-	assert(v(2) == 3);
-
-	Vector3f v2(4, 5, 6);
-
-	v2.print();
-
-	assert(v2(0) == 4);
-	assert(v2(1) == 5);
-	assert(v2(2) == 6);
-
-	return 0;
-}
@@ -56,7 +56,6 @@ px4_add_module(
 		mavlink_rate_limiter.cpp
 		mavlink_receiver.cpp
 		mavlink_ftp.cpp
-		mavlink_params.c
 	DEPENDS
 		platforms__common
 	)
--- a/Show More
+++ b/Show More