Merge branch 'master' into vector_control2

19 files changed, 521 insertions, 516 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
index 8e0914d63..40b2ee68b 100644
--- a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
+++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
@@ -97,9 +97,9 @@ fi
 #
 if [ $MKBLCTRL_FRAME == x ]
 then
-    mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
+    mixer load /dev/mkblctrl /etc/mixers/FMU_quad_x.mix
 else
-    mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
+    mixer load /dev/mkblctrl /etc/mixers/FMU_quad_+.mix
 fi
 
 #
diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index a2d7d371d..cce5e5e54 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -370,14 +370,17 @@ class uploader(object):
                 self.port.close()
                 
         def send_reboot(self):
-                # try reboot via NSH first
-                self.__send(uploader.NSH_INIT)
-                self.__send(uploader.NSH_REBOOT_BL)
-                self.__send(uploader.NSH_INIT)
-                self.__send(uploader.NSH_REBOOT)
-                # then try MAVLINK command
-                self.__send(uploader.MAVLINK_REBOOT_ID1)
-                self.__send(uploader.MAVLINK_REBOOT_ID0)
+                try:
+                    # try reboot via NSH first
+                    self.__send(uploader.NSH_INIT)
+                    self.__send(uploader.NSH_REBOOT_BL)
+                    self.__send(uploader.NSH_INIT)
+                    self.__send(uploader.NSH_REBOOT)
+                    # then try MAVLINK command
+                    self.__send(uploader.MAVLINK_REBOOT_ID1)
+                    self.__send(uploader.MAVLINK_REBOOT_ID0)
+                except:
+                    return
                 
                 
 
diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk
index e1a42530b..5ecf93a3a 100644
--- a/makefiles/config_px4fmu-v1_backside.mk
+++ b/makefiles/config_px4fmu-v1_backside.mk
@@ -69,12 +69,13 @@ MODULES		+= modules/mavlink_onboard
 MODULES		+= modules/gpio_led
 
 #
-# Estimation modules (EKF / other filters)
+# Estimation modules (EKF/ SO3 / other filters)
 #
 #MODULES		+= modules/attitude_estimator_ekf
 MODULES		+= modules/att_pos_estimator_ekf
 #MODULES		+= modules/position_estimator_inav
 MODULES		+= examples/flow_position_estimator
+MODULES		+= modules/attitude_estimator_so3
 
 #
 # Vehicle Control
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index efffa7a32..be24100fc 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -69,9 +69,10 @@ MODULES		+= modules/mavlink_onboard
 MODULES		+= modules/gpio_led
 
 #
-# Estimation modules (EKF / other filters)
+# Estimation modules (EKF/ SO3 / other filters)
 #
 MODULES		+= modules/attitude_estimator_ekf
+MODULES		+= modules/attitude_estimator_so3
 MODULES		+= modules/att_pos_estimator_ekf
 MODULES		+= modules/position_estimator_inav
 MODULES		+= examples/flow_position_estimator
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index e6c81487f..b60a12894 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -68,9 +68,10 @@ MODULES		+= modules/mavlink
 MODULES		+= modules/mavlink_onboard
 
 #
-# Estimation modules (EKF / other filters)
+# Estimation modules (EKF/ SO3 / other filters)
 #
 MODULES		+= modules/attitude_estimator_ekf
+MODULES		+= modules/attitude_estimator_so3
 MODULES		+= modules/att_pos_estimator_ekf
 MODULES		+= modules/position_estimator_inav
 MODULES		+= examples/flow_position_estimator
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 7ef883f94..30d6069b3 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -1,6 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
+ *   Author: Marco Bauer <marco@wtns.de>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,7 +36,8 @@
  * @file mkblctrl.cpp
  *
  * Driver/configurator for the Mikrokopter BL-Ctrl.
- * Marco Bauer
+ *
+ * @author Marco Bauer <marco@wtns.de>
  *
  */
 
@@ -88,8 +90,8 @@
 #define BLCTRL_MIN_VALUE				-0.920F
 #define MOTOR_STATE_PRESENT_MASK		0x80
 #define MOTOR_STATE_ERROR_MASK		0x7F
-#define MOTOR_SPINUP_COUNTER			2500
-#define ESC_UORB_PUBLISH_DELAY			200
+#define MOTOR_SPINUP_COUNTER			30
+#define ESC_UORB_PUBLISH_DELAY		500000
 
 class MK : public device::I2C
 {
@@ -111,7 +113,7 @@ public:
 		FRAME_X,
 	};
 
-	MK(int bus);
+	MK(int bus, const char *_device_path);
 	~MK();
 
 	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
@@ -125,7 +127,7 @@ public:
 	int		set_overrideSecurityChecks(bool overrideSecurityChecks);
 	int		set_px4mode(int px4mode);
 	int		set_frametype(int frametype);
-	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput);
+	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C);
 
 private:
 	static const unsigned _max_actuators = MAX_MOTORS;
@@ -140,6 +142,7 @@ private:
 	unsigned int		_motor;
 	int    _px4mode;
 	int    _frametype;
+	char				_device[20];					///< device
 
 	orb_advert_t	_t_outputs;
 	orb_advert_t	_t_esc_status;
@@ -242,7 +245,7 @@ MK	*g_mk;
 
 } // namespace
 
-MK::MK(int bus) :
+MK::MK(int bus, const char *_device_path) :
 	I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
 	_mode(MODE_NONE),
 	_update_rate(50),
@@ -262,6 +265,10 @@ MK::MK(int bus) :
 	_armed(false),
 	_mixers(nullptr)
 {
+	strncpy(_device, _device_path, sizeof(_device));
+	/* enforce null termination */
+	_device[sizeof(_device) - 1] = '\0';
+
 	_debug_enabled = true;
 }
 
@@ -288,7 +295,7 @@ MK::~MK()
 
 	/* clean up the alternate device node */
 	if (_primary_pwm_device)
-		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+		unregister_driver(_device);
 
 	g_mk = nullptr;
 }
@@ -310,13 +317,15 @@ MK::init(unsigned motors)
 
 	usleep(500000);
 
-	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
-	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+   if (sizeof(_device) > 0) {
+	   ret = register_driver(_device, &fops, 0666, (void *)this);
 
-	if (ret == OK) {
-		log("default PWM output device");
-		_primary_pwm_device = true;
-	}
+	   if (ret == OK) {
+			log("creating alternate output device");
+			_primary_pwm_device = true;
+		}
+
+   }
 
 	/* reset GPIOs */
 	gpio_reset();
@@ -620,10 +629,7 @@ MK::task_main()
 					}
 
 					/* output to BLCtrl's */
-					if (_motortest == true) {
-						mk_servo_test(i);
-
-					} else {
+					if (_motortest != true) {
 						//mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024));	// scale the output to 0 - 1024 and sent to output routine
 						// 11 Bit
 						Motor[i].SetPoint_PX4 = outputs.output[i];
@@ -655,7 +661,7 @@ MK::task_main()
 		 * Only update esc topic every half second.
 		 */
 
-		if (hrt_absolute_time() - esc.timestamp > 500000) {
+		if (hrt_absolute_time() - esc.timestamp > ESC_UORB_PUBLISH_DELAY) {
 			esc.counter++;
 			esc.timestamp = hrt_absolute_time();
 			esc.esc_count = (uint8_t) _num_outputs;
@@ -679,14 +685,21 @@ MK::task_main()
 				esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
 				esc.esc[i].esc_state = (uint16_t) Motor[i].State;
 				esc.esc[i].esc_errorcount = (uint16_t) 0;
+
+   			// if motortest is requested - do it...
+   			if (_motortest == true) {
+	   		   mk_servo_test(i);
+			   }
+
 			}
 
 			orb_publish(ORB_ID(esc_status), _t_esc_status, &esc);
+
 		}
 
 	}
 
-	//::close(_t_esc_status);
+	::close(_t_esc_status);
 	::close(_t_actuators);
 	::close(_t_actuator_armed);
 
@@ -713,8 +726,12 @@ MK::mk_servo_arm(bool status)
 
 
 unsigned int
-MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
+MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
 {
+	if(initI2C) {
+		I2C::init();
+	}
+
 	_retries = 50;
 	uint8_t foundMotorCount = 0;
 
@@ -938,6 +955,7 @@ MK::mk_servo_test(unsigned int chan)
 		if (_motor >= _num_outputs) {
 			_motor = -1;
 			_motortest = false;
+			fprintf(stderr, "[mkblctrl] Motortest finished...\n");
 		}
 	}
 
@@ -1353,7 +1371,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 
 	/* count used motors */
 	do {
-		if (g_mk->mk_check_for_blctrl(8, false) != 0) {
+		if (g_mk->mk_check_for_blctrl(8, false, false) != 0) {
 			shouldStop = 4;
 
 		} else {
@@ -1363,7 +1381,7 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 		sleep(1);
 	} while (shouldStop < 3);
 
-	g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true));
+	g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false));
 
 	/* (re)set the PWM output mode */
 	g_mk->set_mode(servo_mode);
@@ -1376,13 +1394,13 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 }
 
 int
-mk_start(unsigned bus, unsigned motors)
+mk_start(unsigned bus, unsigned motors, char *device_path)
 {
 	int ret = OK;
 
 	if (g_mk == nullptr) {
 
-		g_mk = new MK(bus);
+		g_mk = new MK(bus, device_path);
 
 		if (g_mk == nullptr) {
 			ret = -ENOMEM;
@@ -1401,6 +1419,52 @@ mk_start(unsigned bus, unsigned motors)
 }
 
 
+int
+mk_check_for_i2c_esc_bus(char *device_path, int motors)
+{
+	int ret;
+
+	if (g_mk == nullptr) {
+
+		g_mk = new MK(3, device_path);
+
+		if (g_mk == nullptr) {
+			return -1;
+
+		} else {
+			ret = g_mk->mk_check_for_blctrl(8, false, true);
+			delete g_mk;
+			g_mk = nullptr;
+
+			if (ret > 0) {
+				return 3;
+			}
+
+		}
+
+
+		g_mk = new MK(1, device_path);
+
+		if (g_mk == nullptr) {
+			return -1;
+
+		} else {
+			ret = g_mk->mk_check_for_blctrl(8, false, true);
+			delete g_mk;
+			g_mk = nullptr;
+
+			if (ret > 0) {
+				return 1;
+			}
+
+		}
+	}
+
+	return -1;
+}
+
+
+
 } // namespace
 
 extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
@@ -1411,13 +1475,14 @@ mkblctrl_main(int argc, char *argv[])
 	PortMode port_mode = PORT_FULL_PWM;
 	int pwm_update_rate_in_hz = UPDATE_RATE;
 	int motorcount = 8;
-	int bus = 1;
+	int bus = -1;
 	int px4mode = MAPPING_PX4;
 	int frametype = FRAME_PLUS;	// + plus is default
 	bool motortest = false;
 	bool overrideSecurityChecks = false;
 	bool showHelp = false;
 	bool newMode = false;
+	char *devicepath = "";
 
 	/*
 	 * optional parameters
@@ -1477,36 +1542,69 @@ mkblctrl_main(int argc, char *argv[])
 			newMode = true;
 		}
 
+		/* look for the optional device parameter */
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
+			if (argc > i + 1) {
+				devicepath = argv[i + 1];
+				newMode = true;
+
+			} else {
+				errx(1, "missing the devicename (-d)");
+				return 1;
+			}
+		}
+
 	}
 
 	if (showHelp) {
 		fprintf(stderr, "mkblctrl: help:\n");
-		fprintf(stderr, "  [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n");
-		fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
-		fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n");
-		fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n");
+		fprintf(stderr, "  [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n");
+		fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
+		fprintf(stderr, "\t -b {i2c_bus_number} \t\t Set the i2c bus where the ESCs are connected to (default autoscan).\n");
+		fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n");
 		fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
+		fprintf(stderr, "\n");
+		fprintf(stderr, "Motortest:\n");
+		fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n");
+		fprintf(stderr, "mkblctrl -t\n");
+      fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n");
 		exit(1);
 	}
 
 
-	if (g_mk == nullptr) {
-		if (mk_start(bus, motorcount) != OK) {
-			errx(1, "failed to start the MK-BLCtrl driver");
-
-		} else {
-			newMode = true;
-		}
-	}
-
-
-	/* parameter set ? */
-	if (newMode) {
-		/* switch parameter */
-		return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
-	}
-
-	/* test, etc. here g*/
-
-	exit(1);
+	if (!motortest) {
+   	if (g_mk == nullptr) {
+      	if (bus == -1) {
+      		bus = mk_check_for_i2c_esc_bus(devicepath, motorcount);
+      	}
+
+      	if (bus != -1) {
+     			if (mk_start(bus, motorcount, devicepath) != OK) {
+     				errx(1, "failed to start the MK-BLCtrl driver");
+     			}
+      	} else {
+      		errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)");
+      	}
+
+      	/* parameter set ? */
+      	if (newMode) {
+      		/* switch parameter */
+      		return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
+      	}
+
+      	exit(0);
+      } else {
+         errx(1, "MK-BLCtrl driver already running");
+      }
+
+   } else {
+      if (g_mk == nullptr) {
+         errx(1, "MK-BLCtrl driver not running. You have to start it first.");
+
+      } else {
+         g_mk->set_motor_test(motortest);
+         exit(0);
+
+      }
+   }
 }
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 864a9d24d..a733ef1d2 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -299,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
 
 		// Rate limit PD + FF throttle
 		// Calculate the throttle increment from the specified slew time
-		if (fabsf(_throttle_slewrate) < 0.01f) {
+		if (fabsf(_throttle_slewrate) > 0.01f) {
 			float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
 
 			_throttle_dem = constrain(_throttle_dem,
diff --git a/src/modules/attitude_estimator_so3/README b/src/modules/attitude_estimator_so3/README
new file mode 100644
index 000000000..02ab66ee4
--- /dev/null
+++ b/src/modules/attitude_estimator_so3/README
@@ -0,0 +1,3 @@
+Synopsis
+
+ nsh> attitude_estimator_so3_comp start
\ No newline at end of file
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
index 86bda3c75..e79726613 100755
--- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
@@ -1,16 +1,49 @@
-/*
- * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Hyon Lim <limhyon@gmail.com>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
  *
- * @file attitude_estimator_so3_comp_main.c
+ * 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 attitude_estimator_so3_main.cpp
  *
  * Implementation of nonlinear complementary filters on the SO(3).
  * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
  * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
- * 
+ *
  * Theory of nonlinear complementary filters on the SO(3) is based on [1].
  * Quaternion realization of [1] is based on [2].
  * Optmized quaternion update code is based on Sebastian Madgwick's implementation.
- * 
+ *
  * References
  *  [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
  *  [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
@@ -46,94 +79,91 @@
 #ifdef __cplusplus
 extern "C" {
 #endif
-#include "attitude_estimator_so3_comp_params.h"
+#include "attitude_estimator_so3_params.h"
 #ifdef __cplusplus
 }
 #endif
 
-extern "C" __EXPORT int attitude_estimator_so3_comp_main(int argc, char *argv[]);
+extern "C" __EXPORT int attitude_estimator_so3_main(int argc, char *argv[]);
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;		/**< Deamon status flag */
-static int attitude_estimator_so3_comp_task;				/**< Handle of deamon task / thread */
+static int attitude_estimator_so3_task;				/**< Handle of deamon task / thread */
+
+//! Auxiliary variables to reduce number of repeated operations
 static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;	/** quaternion of sensor frame relative to auxiliary frame */
 static float dq0 = 0.0f, dq1 = 0.0f, dq2 = 0.0f, dq3 = 0.0f;	/** quaternion of sensor frame relative to auxiliary frame */
 static float gyro_bias[3] = {0.0f, 0.0f, 0.0f}; /** bias estimation */
-static bool bFilterInit = false;
-
-//! Auxiliary variables to reduce number of repeated operations
 static float q0q0, q0q1, q0q2, q0q3;
 static float q1q1, q1q2, q1q3;
 static float q2q2, q2q3;
 static float q3q3;
-
-//! Serial packet related
-static int uart;
-static int baudrate;
+static bool bFilterInit = false;
 
 /**
- * Mainloop of attitude_estimator_so3_comp.
+ * Mainloop of attitude_estimator_so3.
  */
-int attitude_estimator_so3_comp_thread_main(int argc, char *argv[]);
+int attitude_estimator_so3_thread_main(int argc, char *argv[]);
 
 /**
  * Print the correct usage.
  */
 static void usage(const char *reason);
 
+/* Function prototypes */
+float invSqrt(float number);
+void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, float mz);
+void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt);
+
 static void
 usage(const char *reason)
 {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
 
-	fprintf(stderr, "usage: attitude_estimator_so3_comp {start|stop|status} [-d <devicename>] [-b <baud rate>]\n"
-		"-d and -b options are for separate visualization with raw data (quaternion packet) transfer\n"
-		"ex) attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200\n");
+	fprintf(stderr, "usage: attitude_estimator_so3 {start|stop|status}\n");
 	exit(1);
 }
 
 /**
- * The attitude_estimator_so3_comp app only briefly exists to start
+ * The attitude_estimator_so3 app only briefly exists to start
  * the background job. The stack size assigned in the
  * Makefile does only apply to this management task.
  *
  * The actual stack size should be set in the call
  * to task_create().
  */
-int attitude_estimator_so3_comp_main(int argc, char *argv[])
+int attitude_estimator_so3_main(int argc, char *argv[])
 {
 	if (argc < 1)
 		usage("missing command");
 
-	
-
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("attitude_estimator_so3_comp already running\n");
+			warnx("already running\n");
 			/* this is not an error */
 			exit(0);
 		}
 
 		thread_should_exit = false;
-		attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp",
+		attitude_estimator_so3_task = task_spawn_cmd("attitude_estimator_so3",
 					      SCHED_DEFAULT,
 					      SCHED_PRIORITY_MAX - 5,
-					      12400,
-					      attitude_estimator_so3_comp_thread_main,
-					      (const char **)argv);
+					      14000,
+					      attitude_estimator_so3_thread_main,
+					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
 		thread_should_exit = true;
 
-		while(thread_running){
+		while (thread_running){
 			usleep(200000);
-			printf(".");
 		}
-		printf("terminated.");
+		
+		warnx("stopped");
 		exit(0);
 	}
 
@@ -157,7 +187,8 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[])
 //---------------------------------------------------------------------------------------------------
 // Fast inverse square-root
 // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
-float invSqrt(float number) {
+float invSqrt(float number) 
+{
     volatile long i;
     volatile float x, y;
     volatile const float f = 1.5F;
@@ -221,48 +252,47 @@ void NonlinearSO3AHRSinit(float ax, float ay, float az, float mx, float my, floa
     q3q3 = q3 * q3;
 }
 
-void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) {
+void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float twoKp, float twoKi, float dt) 
+{
 	float recipNorm;
 	float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
 
-	//! Make filter converge to initial solution faster
-	//! This function assumes you are in static position.
-	//! WARNING : in case air reboot, this can cause problem. But this is very
-	//!	      unlikely happen.
-	if(bFilterInit == false)
-	{
+	// Make filter converge to initial solution faster
+	// This function assumes you are in static position.
+	// WARNING : in case air reboot, this can cause problem. But this is very unlikely happen.
+	if(bFilterInit == false) {
 		NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);
 		bFilterInit = true;
 	}
         	
 	//! If magnetometer measurement is available, use it.
-	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+	if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
 		float hx, hy, hz, bx, bz;
 		float halfwx, halfwy, halfwz;
 	
 		// Normalise magnetometer measurement
 		// Will sqrt work better? PX4 system is powerful enough?
-    		recipNorm = invSqrt(mx * mx + my * my + mz * mz);
-    		mx *= recipNorm;
-    		my *= recipNorm;
-    		mz *= recipNorm;
+    	recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+    	mx *= recipNorm;
+    	my *= recipNorm;
+    	mz *= recipNorm;
     
-    		// Reference direction of Earth's magnetic field
-    		hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
-    		hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
-		hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 - q2q2);
-    		bx = sqrt(hx * hx + hy * hy);
-    		bz = hz;
+    	// Reference direction of Earth's magnetic field
+    	hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
+    	hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
+		hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
+    	bx = sqrt(hx * hx + hy * hy);
+    	bz = hz;
     
-    		// Estimated direction of magnetic field
-    		halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
-    		halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
-    		halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
+    	// Estimated direction of magnetic field
+    	halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
+    	halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
+    	halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
     
-    		// Error is sum of cross product between estimated direction and measured direction of field vectors
-    		halfex += (my * halfwz - mz * halfwy);
-    		halfey += (mz * halfwx - mx * halfwz);
-    		halfez += (mx * halfwy - my * halfwx);
+    	// Error is sum of cross product between estimated direction and measured direction of field vectors
+    	halfex += (my * halfwz - mz * halfwy);
+    	halfey += (mz * halfwx - mx * halfwz);
+    	halfez += (mx * halfwy - my * halfwx);
 	}
 
 	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
@@ -293,7 +323,9 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
 			gyro_bias[0] += twoKi * halfex * dt;	// integral error scaled by Ki
 			gyro_bias[1] += twoKi * halfey * dt;
 			gyro_bias[2] += twoKi * halfez * dt;
-			gx += gyro_bias[0];	// apply integral feedback
+			
+			// apply integral feedback
+			gx += gyro_bias[0];
 			gy += gyro_bias[1];
 			gz += gyro_bias[2];
 		}
@@ -337,208 +369,43 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
 	q3 *= recipNorm;
 
 	// Auxiliary variables to avoid repeated arithmetic
-        q0q0 = q0 * q0;
-       	q0q1 = q0 * q1;
-       	q0q2 = q0 * q2;
-       	q0q3 = q0 * q3;
-       	q1q1 = q1 * q1;
-       	q1q2 = q1 * q2;
+    q0q0 = q0 * q0;
+    q0q1 = q0 * q1;
+    q0q2 = q0 * q2;
+    q0q3 = q0 * q3;
+    q1q1 = q1 * q1;
+    q1q2 = q1 * q2;
    	q1q3 = q1 * q3;
-       	q2q2 = q2 * q2;
-       	q2q3 = q2 * q3;
-       	q3q3 = q3 * q3;   
-}
-
-void send_uart_byte(char c)
-{
-	write(uart,&c,1);
-}
-
-void send_uart_bytes(uint8_t *data, int length)
-{
-	write(uart,data,(size_t)(sizeof(uint8_t)*length));
-}
-
-void send_uart_float(float f) {
-  uint8_t * b = (uint8_t *) &f;
-
-  //! Assume float is 4-bytes
-  for(int i=0; i<4; i++) {
-    
-    uint8_t b1 = (b[i] >> 4) & 0x0f;
-    uint8_t b2 = (b[i] & 0x0f);
-    
-    uint8_t c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
-    uint8_t c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;
-    
-    send_uart_bytes(&c1,1);
-    send_uart_bytes(&c2,1);
-  }
-}
-
-void send_uart_float_arr(float *arr, int length)
-{
-	for(int i=0;i<length;++i)
-	{
-		send_uart_float(arr[i]);
-		send_uart_byte(',');
-	}
-}
-
-int open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
-{
-	int speed;
-	
-	switch (baud) {
-        case 0:      speed = B0;      break;
-        case 50:     speed = B50;     break;
-        case 75:     speed = B75;     break;
-        case 110:    speed = B110;    break;
-        case 134:    speed = B134;    break;
-        case 150:    speed = B150;    break;
-        case 200:    speed = B200;    break;
-        case 300:    speed = B300;    break;
-        case 600:    speed = B600;    break;
-        case 1200:   speed = B1200;   break;
-        case 1800:   speed = B1800;   break;
-        case 2400:   speed = B2400;   break;
-        case 4800:   speed = B4800;   break;
-        case 9600:   speed = B9600;   break;
-        case 19200:  speed = B19200;  break;
-        case 38400:  speed = B38400;  break;
-        case 57600:  speed = B57600;  break;
-        case 115200: speed = B115200; break;
-        case 230400: speed = B230400; break;
-        case 460800: speed = B460800; break;
-        case 921600: speed = B921600; break;
-        default:
-                printf("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
-                return -EINVAL;
-        }
-
-	printf("[so3_comp_filt] UART is %s, baudrate is %d\n", uart_name, baud);
-        uart = open(uart_name, O_RDWR | O_NOCTTY);
-
-	/* Try to set baud rate */
-        struct termios uart_config;
-        int termios_state;
-        *is_usb = false;
-
-	/* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */
-        if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) {
-                /* Back up the original uart configuration to restore it after exit */
-                if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-                        printf("ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
-                        close(uart);
-                        return -1;
-                }
-
-                /* Fill the struct for the new configuration */
-                tcgetattr(uart, &uart_config);
-
-                /* Clear ONLCR flag (which appends a CR for every LF) */
-                uart_config.c_oflag &= ~ONLCR;
-
-                /* Set baud rate */
-                if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-                        printf("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
-                        close(uart);
-                        return -1;
-                }
-
-
-                if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-                        printf("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
-                        close(uart);
-                        return -1;
-                }
-
-        } else {
-                *is_usb = true;
-        }
-
-        return uart;
+    q2q2 = q2 * q2;
+    q2q3 = q2 * q3;
+    q3q3 = q3 * q3;   
 }
 
 /*
- * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
- */
-
-/*
- * EKF Attitude Estimator main function.
+ * Nonliner complementary filter on SO(3), attitude estimator main function.
  *
- * Estimates the attitude recursively once started.
+ * Estimates the attitude once started.
  *
  * @param argc number of commandline arguments (plus command name)
  * @param argv strings containing the arguments
  */
-int attitude_estimator_so3_comp_thread_main(int argc, char *argv[])
+int attitude_estimator_so3_thread_main(int argc, char *argv[])
 {
-
-const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
-
-	//! Serial debug related
-	int ch;
-	struct termios uart_config_original;
-	bool usb_uart;
-	bool debug_mode = false;
-	char *device_name = "/dev/ttyS2";
-	baudrate = 115200;
+	const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	//! Time constant
 	float dt = 0.005f;
 	
 	/* output euler angles */
 	float euler[3] = {0.0f, 0.0f, 0.0f};
-
-	float Rot_matrix[9] = {1.f,  0,  0,
-			      0,  1.f,  0,
-			      0,  0,  1.f
-			     };		/**< init: identity matrix */
-
+	
+	/* Initialization */
+	float Rot_matrix[9] = {1.f,  0.0f,  0.0f, 0.0f,  1.f,  0.0f, 0.0f,  0.0f,  1.f };		/**< init: identity matrix */
 	float acc[3] = {0.0f, 0.0f, 0.0f};
 	float gyro[3] = {0.0f, 0.0f, 0.0f};
 	float mag[3] = {0.0f, 0.0f, 0.0f};
 
-	/* work around some stupidity in task_create's argv handling */
-	argc -= 2;
-	argv += 2;
-
-	//! -d <device_name>, default : /dev/ttyS2
-	//! -b <baud_rate>,   default : 115200
-	while ((ch = getopt(argc,argv,"d:b:")) != EOF){
-		switch(ch){
-			case 'b':
-				baudrate = strtoul(optarg, NULL, 10);
-				if(baudrate == 0)
-					printf("invalid baud rate '%s'",optarg);
-				break;
-			case 'd':
-				device_name = optarg;
-				debug_mode = true;
-				break;
-			default:
-				usage("invalid argument");
-		}
-	}
-
-	if(debug_mode){
-		printf("Opening debugging port for 3D visualization\n");
-		uart = open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
-		if (uart < 0)
-                	printf("could not open %s", device_name);
-		else
-			printf("Open port success\n");
-	}
-
-	// print text
-	printf("Nonlinear SO3 Attitude Estimator initialized..\n\n");
-	fflush(stdout);
-
-	int overloadcounter = 19;
-
-	/* store start time to guard against too slow update rates */
-	uint64_t last_run = hrt_absolute_time();
+	warnx("main thread started");
 
 	struct sensor_combined_s raw;
 	memset(&raw, 0, sizeof(raw));
@@ -555,8 +422,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	/* subscribe to raw data */
 	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
-	/* rate-limit raw data updates to 200Hz */
-	orb_set_interval(sub_raw, 4);
+	/* rate-limit raw data updates to 333 Hz (sensors app publishes at 200, so this is just paranoid) */
+	orb_set_interval(sub_raw, 3);
 
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
@@ -565,17 +432,15 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 
 	/* advertise attitude */
-	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+	//orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+	//orb_advert_t att_pub = -1;
+	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
 	int loopcounter = 0;
 	int printcounter = 0;
 
 	thread_running = true;
 
-	/* advertise debug value */
-	// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
-	// orb_advert_t pub_dbg = -1;
-
 	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
 	// XXX write this out to perf regs
 
@@ -583,20 +448,22 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	uint32_t sensor_last_count[3] = {0, 0, 0};
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
-	struct attitude_estimator_so3_comp_params so3_comp_params;
-	struct attitude_estimator_so3_comp_param_handles so3_comp_param_handles;
+	struct attitude_estimator_so3_params so3_comp_params;
+	struct attitude_estimator_so3_param_handles so3_comp_param_handles;
 
 	/* initialize parameter handles */
 	parameters_init(&so3_comp_param_handles);
+	parameters_update(&so3_comp_param_handles, &so3_comp_params);
 
 	uint64_t start_time = hrt_absolute_time();
 	bool initialized = false;
+	bool state_initialized = false;
 
 	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
 	unsigned offset_count = 0;
 
 	/* register the perf counter */
-	perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3_comp");
+	perf_counter_t so3_comp_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_so3");
 
 	/* Main loop*/
 	while (!thread_should_exit) {
@@ -615,12 +482,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
 
 			if (!control_mode.flag_system_hil_enabled) {
-				fprintf(stderr,
-					"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
+				warnx("WARNING: Not getting sensors - sensor app running?");
 			}
-
 		} else {
-
 			/* only update parameters if they changed */
 			if (fds[1].revents & POLLIN) {
 				/* read from param to clear updated flag */
@@ -644,11 +508,12 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					gyro_offsets[2] += raw.gyro_rad_s[2];
 					offset_count++;
 
-					if (hrt_absolute_time() - start_time > 3000000LL) {
+					if (hrt_absolute_time() > start_time + 3000000l) {
 						initialized = true;
 						gyro_offsets[0] /= offset_count;
 						gyro_offsets[1] /= offset_count;
 						gyro_offsets[2] /= offset_count;
+						warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]);
 					}
 
 				} else {
@@ -668,9 +533,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
 
-					gyro[0] =  raw.gyro_rad_s[0] - gyro_offsets[0];
-					gyro[1] =  raw.gyro_rad_s[1] - gyro_offsets[1];
-					gyro[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];
+					gyro[0] = raw.gyro_rad_s[0] - gyro_offsets[0];
+					gyro[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
+					gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
 					if (sensor_last_count[1] != raw.accelerometer_counter) {
@@ -696,31 +561,14 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					mag[1] = raw.magnetometer_ga[1];
 					mag[2] = raw.magnetometer_ga[2];
 
-					uint64_t now = hrt_absolute_time();
-					unsigned int time_elapsed = now - last_run;
-					last_run = now;
-
-					if (time_elapsed > loop_interval_alarm) {
-						//TODO: add warning, cpu overload here
-						// if (overloadcounter == 20) {
-						// 	printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
-						// 	overloadcounter = 0;
-						// }
-
-						overloadcounter++;
-					}
-
-					static bool const_initialized = false;
-
 					/* initialize with good values once we have a reasonable dt estimate */
-					if (!const_initialized && dt < 0.05f && dt > 0.005f) {
-						dt = 0.005f;
-						parameters_update(&so3_comp_param_handles, &so3_comp_params);
-						const_initialized = true;
+					if (!state_initialized && dt < 0.05f && dt > 0.001f) {
+						state_initialized = true;
+						warnx("state initialized");
 					}
 
 					/* do not execute the filter if not initialized */
-					if (!const_initialized) {
+					if (!state_initialized) {
 						continue;
 					}
 
@@ -728,18 +576,23 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 					// NOTE : Accelerometer is reversed.
 					// Because proper mount of PX4 will give you a reversed accelerometer readings.
-					NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt);
+					NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2],
+										-acc[0], -acc[1], -acc[2],
+										mag[0], mag[1], mag[2],
+										so3_comp_params.Kp,
+										so3_comp_params.Ki, 
+										dt);
 
 					// Convert q->R, This R converts inertial frame to body frame.
 					Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
-        				Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3);	// 12
-        				Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2);	// 13
-        				Rot_matrix[3] = 2.0 * (q1*q2 - q0*q3);	// 21
-        				Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
-        				Rot_matrix[5] = 2.0 * (q2*q3 + q0*q1);	// 23
-        				Rot_matrix[6] = 2.0 * (q1*q3 + q0*q2);	// 31
-        				Rot_matrix[7] = 2.0 * (q2*q3 - q0*q1);	// 32
-        				Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
+					Rot_matrix[1] = 2.f * (q1*q2 + q0*q3);	// 12
+					Rot_matrix[2] = 2.f * (q1*q3 - q0*q2);	// 13
+					Rot_matrix[3] = 2.f * (q1*q2 - q0*q3);	// 21
+					Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;// 22
+					Rot_matrix[5] = 2.f * (q2*q3 + q0*q1);	// 23
+					Rot_matrix[6] = 2.f * (q1*q3 + q0*q2);	// 31
+					Rot_matrix[7] = 2.f * (q2*q3 - q0*q1);	// 32
+					Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;// 33
 
 					//1-2-3 Representation.
 					//Equation (290) 
@@ -747,29 +600,42 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					// Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix.
 					euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]);	//! Roll
 					euler[1] = -asinf(Rot_matrix[2]);	//! Pitch
-					euler[2] = atan2f(Rot_matrix[1],Rot_matrix[0]);		//! Yaw
+					euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]);		//! Yaw
 					
 					/* swap values for next iteration, check for fatal inputs */
 					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
-						/* Do something */
+						// Publish only finite euler angles
+						att.roll = euler[0] - so3_comp_params.roll_off;
+						att.pitch = euler[1] - so3_comp_params.pitch_off;
+						att.yaw = euler[2] - so3_comp_params.yaw_off;
 					} else {
 						/* due to inputs or numerical failure the output is invalid, skip it */
+						// Due to inputs or numerical failure the output is invalid
+						warnx("infinite euler angles, rotation matrix:");
+						warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]);
+						warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]);
+						warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]);
+						// Don't publish anything
 						continue;
 					}
 
-					if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator so3_comp] sensor data missed! (%llu)\n", raw.timestamp - last_data);
+					if (last_data > 0 && raw.timestamp > last_data + 12000) {
+						warnx("sensor data missed");
+					}
 
 					last_data = raw.timestamp;
 
 					/* send out */
 					att.timestamp = raw.timestamp;
+					
+					// Quaternion
+					att.q[0] = q0;
+					att.q[1] = q1;
+					att.q[2] = q2;
+					att.q[3] = q3;
+					att.q_valid = true;
 
-					// XXX Apply the same transformation to the rotation matrix
-					att.roll = euler[0] - so3_comp_params.roll_off;
-					att.pitch = euler[1] - so3_comp_params.pitch_off;
-					att.yaw = euler[2] - so3_comp_params.yaw_off;
-
-					//! Euler angle rate. But it needs to be investigated again.
+					// Euler angle rate. But it needs to be investigated again.
 					/*
 					att.rollspeed = 2.0f*(-q1*dq0 + q0*dq1 - q3*dq2 + q2*dq3);
 					att.pitchspeed = 2.0f*(-q2*dq0 + q3*dq1 + q0*dq2 - q1*dq3);
@@ -783,53 +649,30 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					att.pitchacc = 0;
 					att.yawacc = 0;
 
-					//! Quaternion
-					att.q[0] = q0;
-					att.q[1] = q1;
-					att.q[2] = q2;
-					att.q[3] = q3;
-					att.q_valid = true;
-
 					/* TODO: Bias estimation required */
 					memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
 
 					/* copy rotation matrix */
 					memcpy(&att.R, Rot_matrix, sizeof(float)*9);
 					att.R_valid = true;
-
-					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
-						// Broadcast
-						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
-
+					
+					// Publish
+					if (att_pub > 0) {
+						orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
 					} else {
 						warnx("NaN in roll/pitch/yaw estimate!");
+						 orb_advertise(ORB_ID(vehicle_attitude), &att);
 					}
 
 					perf_end(so3_comp_loop_perf);
-
-					//! This will print out debug packet to visualization software
-					if(debug_mode)
-					{
-						float quat[4];
-						quat[0] = q0;
-						quat[1] = q1;
-						quat[2] = q2;
-						quat[3] = q3;
-						send_uart_float_arr(quat,4);
-						send_uart_byte('\n');
-					}
 				}
 			}
 		}
 
 		loopcounter++;
-	}// while
+	}
 
 	thread_running = false;
 
-	/* Reset the UART flags to original state */
-        if (!usb_uart)
-                tcsetattr(uart, TCSANOW, &uart_config_original);
-
 	return 0;
 }
diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c
new file mode 100755
index 000000000..0c8d522b4
--- /dev/null
+++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.c
@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Hyon Lim <limhyon@gmail.com>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *
+ * 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 attitude_estimator_so3_params.c
+ *
+ * Parameters for nonlinear complementary filters on the SO(3).
+ */
+
+#include "attitude_estimator_so3_params.h"
+
+/* This is filter gain for nonlinear SO3 complementary filter */
+/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
+   Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
+   If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
+   will compensate gyro bias which depends on temperature and vibration of your vehicle */
+PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
+                                       //! You can set this gain higher if you want more fast response.
+                                       //! But note that higher gain will give you also higher overshoot.
+PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
+					//! This gain is depend on your vehicle status.
+
+/* offsets in roll, pitch and yaw of sensor plane and body */
+PARAM_DEFINE_FLOAT(SO3_ROLL_OFFS, 0.0f);
+PARAM_DEFINE_FLOAT(SO3_PITCH_OFFS, 0.0f);
+PARAM_DEFINE_FLOAT(SO3_YAW_OFFS, 0.0f);
+
+int parameters_init(struct attitude_estimator_so3_param_handles *h)
+{
+	/* Filter gain parameters */
+	h->Kp = 	param_find("SO3_COMP_KP");
+	h->Ki = 	param_find("SO3_COMP_KI");
+
+	/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
+	h->roll_off  =	param_find("SO3_ROLL_OFFS");
+	h->pitch_off =	param_find("SO3_PITCH_OFFS");
+	h->yaw_off   =	param_find("SO3_YAW_OFFS");
+
+	return OK;
+}
+
+int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p)
+{
+	/* Update filter gain */
+	param_get(h->Kp, &(p->Kp));
+	param_get(h->Ki, &(p->Ki));
+
+	/* Update attitude offset */
+	param_get(h->roll_off, &(p->roll_off));
+	param_get(h->pitch_off, &(p->pitch_off));
+	param_get(h->yaw_off, &(p->yaw_off));
+
+	return OK;
+}
diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h
new file mode 100755
index 000000000..dfb4cad05
--- /dev/null
+++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_params.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Hyon Lim <limhyon@gmail.com>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *
+ * 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 attitude_estimator_so3_params.h
+ *
+ * Parameters for nonlinear complementary filters on the SO(3).
+ */
+
+#include <systemlib/param/param.h>
+
+struct attitude_estimator_so3_params {
+	float Kp;
+	float Ki;
+	float roll_off;
+	float pitch_off;
+	float yaw_off;
+};
+
+struct attitude_estimator_so3_param_handles {
+	param_t Kp, Ki;
+	param_t roll_off, pitch_off, yaw_off;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct attitude_estimator_so3_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct attitude_estimator_so3_param_handles *h, struct attitude_estimator_so3_params *p);
diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk
new file mode 100644
index 000000000..e29bb16a6
--- /dev/null
+++ b/src/modules/attitude_estimator_so3/module.mk
@@ -0,0 +1,8 @@
+#
+# Attitude estimator (Nonlinear SO(3) complementary Filter)
+#
+
+MODULE_COMMAND	 = attitude_estimator_so3
+
+SRCS		 = attitude_estimator_so3_main.cpp \
+		   attitude_estimator_so3_params.c
diff --git a/src/modules/attitude_estimator_so3_comp/README b/src/modules/attitude_estimator_so3_comp/README
deleted file mode 100644
index 79c50a531..000000000
--- a/src/modules/attitude_estimator_so3_comp/README
+++ /dev/null
@@ -1,5 +0,0 @@
-Synopsis
-
- nsh> attitude_estimator_so3_comp start -d /dev/ttyS1 -b 115200
-
-Option -d is for debugging packet. See code for detailed packet structure.
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c
deleted file mode 100755
index f962515df..000000000
--- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.c
+++ /dev/null
@@ -1,63 +0,0 @@
-/*
- * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
- *
- * @file attitude_estimator_so3_comp_params.c
- *
- * Implementation of nonlinear complementary filters on the SO(3).
- * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
- * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
- * 
- * Theory of nonlinear complementary filters on the SO(3) is based on [1].
- * Quaternion realization of [1] is based on [2].
- * Optmized quaternion update code is based on Sebastian Madgwick's implementation.
- * 
- * References
- *  [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
- *  [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
- */
-
-#include "attitude_estimator_so3_comp_params.h"
-
-/* This is filter gain for nonlinear SO3 complementary filter */
-/* NOTE : How to tune the gain? First of all, stick with this default gain. And let the quad in stable place.
-   Log the steady state reponse of filter. If it is too slow, increase SO3_COMP_KP.
-   If you are flying from ground to high altitude in short amount of time, please increase SO3_COMP_KI which
-   will compensate gyro bias which depends on temperature and vibration of your vehicle */
-PARAM_DEFINE_FLOAT(SO3_COMP_KP, 1.0f); //! This parameter will give you about 15 seconds convergence time.
-                                       //! You can set this gain higher if you want more fast response.
-                                       //! But note that higher gain will give you also higher overshoot.
-PARAM_DEFINE_FLOAT(SO3_COMP_KI, 0.05f); //! This gain will incorporate slow time-varying bias (e.g., temperature change)
-					//! This gain is depend on your vehicle status.
-
-/* offsets in roll, pitch and yaw of sensor plane and body */
-PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
-PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
-
-int parameters_init(struct attitude_estimator_so3_comp_param_handles *h)
-{
-	/* Filter gain parameters */
-	h->Kp = 	param_find("SO3_COMP_KP");
-	h->Ki = 	param_find("SO3_COMP_KI");
-
-	/* Attitude offset (WARNING: Do not change if you do not know what exactly this variable wil lchange) */
-	h->roll_off  =	param_find("ATT_ROLL_OFFS");
-	h->pitch_off =	param_find("ATT_PITCH_OFFS");
-	h->yaw_off   =	param_find("ATT_YAW_OFFS");
-
-	return OK;
-}
-
-int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p)
-{
-	/* Update filter gain */
-	param_get(h->Kp, &(p->Kp));
-	param_get(h->Ki, &(p->Ki));
-
-	/* Update attitude offset */
-	param_get(h->roll_off, &(p->roll_off));
-	param_get(h->pitch_off, &(p->pitch_off));
-	param_get(h->yaw_off, &(p->yaw_off));
-
-	return OK;
-}
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h
deleted file mode 100755
index f00695630..000000000
--- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_params.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*
- * Author: Hyon Lim <limhyon@gmail.com, hyonlim@snu.ac.kr>
- *
- * @file attitude_estimator_so3_comp_params.h
- *
- * Implementation of nonlinear complementary filters on the SO(3).
- * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
- * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
- * 
- * Theory of nonlinear complementary filters on the SO(3) is based on [1].
- * Quaternion realization of [1] is based on [2].
- * Optmized quaternion update code is based on Sebastian Madgwick's implementation.
- * 
- * References
- *  [1] Mahony, R.; Hamel, T.; Pflimlin, Jean-Michel, "Nonlinear Complementary Filters on the Special Orthogonal Group," Automatic Control, IEEE Transactions on , vol.53, no.5, pp.1203,1218, June 2008
- *  [2] Euston, M.; Coote, P.; Mahony, R.; Jonghyuk Kim; Hamel, T., "A complementary filter for attitude estimation of a fixed-wing UAV," Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on , vol., no., pp.340,345, 22-26 Sept. 2008
- */
-
-#include <systemlib/param/param.h>
-
-struct attitude_estimator_so3_comp_params {
-	float Kp;
-	float Ki;
-	float roll_off;
-	float pitch_off;
-	float yaw_off;
-};
-
-struct attitude_estimator_so3_comp_param_handles {
-	param_t Kp, Ki;
-	param_t roll_off, pitch_off, yaw_off;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-int parameters_init(struct attitude_estimator_so3_comp_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-int parameters_update(const struct attitude_estimator_so3_comp_param_handles *h, struct attitude_estimator_so3_comp_params *p);
diff --git a/src/modules/attitude_estimator_so3_comp/module.mk b/src/modules/attitude_estimator_so3_comp/module.mk
deleted file mode 100644
index 92f43d920..000000000
--- a/src/modules/attitude_estimator_so3_comp/module.mk
+++ /dev/null
@@ -1,8 +0,0 @@
-#
-# Attitude estimator (Nonlinear SO3 complementary Filter)
-#
-
-MODULE_COMMAND	 = attitude_estimator_so3_comp
-
-SRCS		 = attitude_estimator_so3_comp_main.cpp \
-		   attitude_estimator_so3_comp_params.c
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ace13bb78..dfd4d2f73 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -871,7 +871,7 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
-		if (status.condition_local_altitude_valid) {
+		if (status.is_rotary_wing && status.condition_local_altitude_valid) {
 			if (status.condition_landed != local_position.landed) {
 				status.condition_landed = local_position.landed;
 				status_changed = true;
@@ -1539,7 +1539,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 			// TODO AUTO_LAND handling
 			if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
 				/* don't switch to other states until takeoff not completed */
-				if (local_pos->z > -takeoff_alt || status->condition_landed) {
+				// XXX: only respect the condition_landed when the local position is actually valid
+				if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
 					return TRANSITION_NOT_CHANGED;
 				}
 			}
@@ -1549,7 +1550,7 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 			    status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
 			    status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
 				/* possibly on ground, switch to TAKEOFF if needed */
-				if (local_pos->z > -takeoff_alt || status->condition_landed) {
+				if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) {
 					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
 					return res;
 				}
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index 97aa275de..be76524da 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -33,9 +33,9 @@
  ****************************************************************************/
 
 /**
- * @file fw_pos_control_l1_params.c
+ * @file fw_att_control_params.c
  *
- * Parameters defined by the L1 position control task
+ * Parameters defined by the fixed-wing attitude control task
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 241ee558a..c37c35a63 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -240,7 +240,7 @@ l_vehicle_attitude(const struct listener *l)
 					  att.rollspeed,
 					  att.pitchspeed,
 					  att.yawspeed);
-
+					  	
 		/* limit VFR message rate to 10Hz */
 		hrt_abstime t = hrt_absolute_time();
 		if (t >= last_sent_vfr + 100000) {
@@ -250,6 +250,19 @@ l_vehicle_attitude(const struct listener *l)
 			float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
 			mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
 		}
+		
+		/* send quaternion values if it exists */
+		if(att.q_valid) {
+			mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
+												last_sensor_timestamp / 1000,
+												att.q[0],
+												att.q[1],
+												att.q[2],
+												att.q[3],
+												att.rollspeed,
+												att.pitchspeed,
+												att.yawspeed);
+		}
 	}
 
 	attitude_counter++;